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Section 1 
INTRODUCTION 

This report describes NAVSIM II, a computer program for 
analytical simulation of an aided-inertial navigation system for 
aircraft — and, in particular, for helicopters. This program 
was developed for NASA-Ames Research Center by Analytical 
Mechanics Associates, Inc. (AMA) as one of the tasks under 
contract NAS2-10850. This report comprises the final report for 
that contract . 

As its name implies, NAVSIM II is a second-generation navigation 
simulation, superceding an earlier program, NAVSIM, which has 
been used by NASA/ARC's Guidance and Navigation Branch in support 
of STOLAND and V/STOLAND flight tests. NAVSIM II was developed 
independently of NAVSIM, however, using structured design 
principles and techniques to produce a "clean" program which can 
be easily understood, modified and extended. The resulting 
software is functionally modular, reasonably uncoupled, 
well-commented and written in fairly standard Fortran 77 for 
ready transportability. Care was taken in the design to minimize 
the user's workload and confusion. A significant goal of the 
design and development effort was to produce not just a program 
for solving a particular problem, but rather the core software 
for application to solution of many future problems through 
extension and modification. 

NAVSIM II simulates a strapped-down inertial navigation system 
which may be aided by measurements of: 

* GPS pseudo-range 

* barometric and/or radio altitude 

* Doppler radar (velocity) 

* airspeed 

* position (fixes). 

Aiding is implemented by processing these measurements through an 
extended Kalman filter. Bierman's U-D procedure [1] is used in 
the filter implementation. Concern is taken for some of the 
considerations of real-system implementation (processing order, 
data screening), but the Fortran simulation is not constrained by 
such common real-system considerations as fixed-point arithmetic, 
allowable cycle time and word length. 
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Figure 1 presents an overview of the simulation process 
implemented in NAVSIM II. The "actual trajectory" generator 
(TAFCOS ) provides: 

* initial position, velocity and attitude 
for initiating the integration of the 
estimated navigation equations 

* specific forces and body rates to simulate 
the measured accelerometer and gyro outputs 
of the strapped-down IMU 

* position and velocity vectors to simulate 
the "true" measurements from the navaids. 

The IMU model adds simulated sensor errors to the "actual" 
specific forces and body rates to form the simulated 
inertial-sensor outputs. These outputs are the inputs to the 
navigation equations (integration of the differential equations 
for position, velocity and attitude). Vhen measurements are 
simulated, the estimated position and velocity and the "actual" 
position and velocity are used by the various measurement 
simulators to form the "actual" and estimated measurements. The 
pre-processor logic in each of these modules produces: 

* the measurement residual 

* the sensitivity (partial) of the measurement 
to filter-state variations 

* the filter weight (user-supplied). 

These residuals, sensitivities and weights are processed by the 
Kalman filter to produce new values of the estimation errors 
(i.e., the filter-states). These values are added to the 
previous estimates to form new estimates of position, velocity, 
tilts and inertial-sensor bias compensations (all used in the 
navigation equations) and new estimates of measurement bias 
compensation (used in measurement pre-processing). 

The next sections describe the hardware /software configuration 
simulated by NAVSIM II, the program's application to aircraft 
navigation investigations and procedures for utilization of the 
program. Later sections are devoted to documentation of the 
software, discussion of future extensions and recommendations for 
program enhancement. Five appendices are included. The first of 
these presents the mathematical formulation (without derivation). 
The second contains procedures and examples for program 
modification. The third discusses the utility software for 
plotting and for specification of the reference flight path. The 
last two appendices contain collections of subroutine summaries 
and commons descriptions. 
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figure 1: NAVSIM II Simulation Process 






Section II 

THE SIMULATED SITUATION 

NAVSIM II simulates an aided-inertial navigation system flown 
aboard a helicopter. Its equations and logical design represent 
a certain range of hardware, software and vehicle configurations. 
It may be used to simulate navigation during several different 
mission scenarios. 


Configuration 

The vehicle simulated in NAVSIM II is considered to be a 
helicopter or other rotorcraft . Nothing in the navigational 
logic prevents the vehicle from being considered as a CTOL 
aircraft, however. It is only the flight path, attitude and rate 
information which are generated for the simulated "true" aircraft 
which restrict the interpretation. These variables are generated 
within NAVSIM II according to a realistic model of the UH-1H 
helicopter flying under control of the TAFCOS [2] guidance and 
automatic flight control system. 

The simulation assumes that the airborne hardware includes: 

(1) a strapped-down inertial measurement unit (IMU) which 
includes a triad of inertial-grade laser gyros and a 
triad of integrating accelerometers, 

(2) a navigation computer interfaced with the IMU, the 
various external-measurement devices and a panel or 
other display device , 

(3) a GPS receiver interfaced with the navigation computer, 

(4) a Doppler radar receiver interfaced with the navigation 
computer , 

(5) barometric and radio altimeters interfaced with the 
navigation computer, 

(6) an airspeed indicator interfaced to the navigation 
computer . 

(7) a device capable of displaying the navigational results 
and accepting mode controls or other data from the 
navigator . 

The airborne software represented in NAVSIM II is that which 
is necessary to implement an aided-inertial strapped-down 
navigation system: namely, software to perform the free-inertial 

strapped-down navigation function, Kalman filtering software to 
perform the estimation (aiding) function, and interfacing 
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software for communicating between the navigation computer and 
the various external devices. It should be noted that the 
airborne navigation software would probably differ considerably 
from that found in NAVSIM II. The simulation is not subject to 
the time and storage constraints which are found in an airborne 
implementation, nor is it subject to the variety of problems 
which inevitably arise in the use of real data and real sensors. 


Mission Scenarios 

The missions which have been considered in the design of 
NAVSIM II are those civilian missions which might be undertaken 
by a rotorcraft equipped with the navigational hardware and 
software described above. Such missions include 
search-and-rescue , police surveillance, oil /gas exploration, land 
management and remote-site transportation, as well as 
airport-to-airport operation. 

The user of NAVSIM II may choose to simulate navigation on 
any or all portions of a helicopter mission, although the 
terminal-area portion is generally of the most interest in 
navigation and guidance analyses. In its simplest form, a 
mission consists of a departure phase, an en-route phase and an 
approach-and-landing phase . When the navigation system is 
inertial, the departure phase must be preceded by an alignment 
phase during which initial conditions are initialized on 
position, velocity and attitude estimates in the navigation 
computations. This alignment phase takes place while the craft 
is stationary. Transition to the "navigate" mode can be 
initiated either manually (via the control panel) or 
automatically, using some criterion programmed into the 
navigation computer. During flight to the destination, the 
navigation estimate is continually updated (i.e., computed) 
through integration of the inertial navigation equations and 
aided through processing of the various external measurements by 
the Kalman filter logic. The particular mixture of external 
measurements to be included would usually be determined by a 
measurement schedule specified by the navigator before and/or 
during the flight . 
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Applications 


NAVSIM II is written as a tool which may he used, by a 
knowledgeable engineer to study the effects of the various 
aided-navigation system parameters. With this tool, he can: 

* design/conf igure airborne systems 

* plan research flight projects 

* analyze reported results of real or simulated 
navigation systems 

* perform analytical studies of hypothetical configurations. 

This tool may also be used as an educational device for exhibiting 
the effects of the various parameters on system performance. 

Among the parameters the user may vary are: 

* inertial system errors such as gyro drifts and 
accelerometer null biases 

* measurement errors 

* initial condition errors 

* filter weights 

* process noise 

* measurement schedules 

* flightpath. 
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Section III 
UTILIZATION 

NAVSIM II resides on the MARS Vax 11/780 computer at NASA/ARC. 
It requires two input files and it produces seven output files in 
addition to the system output file (log file). These are shown 
in Figure 2. Both input and output files are self-documenting. 

That is, the input numbers are all titled within the files and 
the input files are identified by a header. The output files are 
tagged with the input header and dated and paged automatically. 
Output formats are columnar with explicit headings. 

The input files are prepared by the user by means of a 
system editing utility (e.g., EDT) before running NAVSIM II. 

Formats and contents of these files will be described later in 
this section. 

The program is invoked (executed) interactively by typing 

RUN NAVSIM 

in response to the operating system's prompt. The user will 
then be prompted by 

enter navigation file name : 

He must enter the name of the navigation-parameter input file 
name, complete with extension. The program will then prompt 

enter trajectory file name: 

and this name must be provided. The program will then proceed 
into execution, not pausing or interacting until completion of 
its scheduled calculations. System output will be delivered to 
the terminal as it is produced. This information is not of much 
general interest . The normal output of interest will be found in 
the other files. The user may TYPE, print or edit these files to 
view their contents. Each file produced has the principal name 
of the input navigation-parameter file as supplied by the user in 
response to the first prompt from NAVSIM II. The extensions 
(.OIN, . ER1, . ER2 , .SGI. . SG2 , .RES, . TR J ) are appended by NAVSIM 
II to identify the contents. 

NAVSIM II may also be run in the batch mode by entering 

@NAVSM 

to invoke a command procedure. This procedure will prompt the 
user for the navigation-parameter and trajectory input-file 
names. Finally, the user is prompted for which batch queue he 
wishes (SUBMIT, MBAT, LBAT) . System output is written to a 
log-file of which the primary filename is that of the 
navigation-parameter file (e.g., NAVXXX.LOG). 
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Input Files 


Navigation Parameters 


The navigation-parameter file (e.g. , NAVXXX.INP) has a 
structured format which can he changed only with care and 
recompilation. This format is shown in Figure 3. Observe first 
that the first line-entry is a header line. This line becomes 
the user's identification in the output files which will be 
produced by running NAVSIM II with this input file. The 
remaining parameters to be specified are functionally grouped: 

Group Function 

***** ******** 

1 Run scheduling and Monte Carlo treatment 

2 Measurement schedules 

3 Navigation data (i.e., filter parameters) 

4 Simulated errors 

5 GPS satellite information 

6 Runway reference. 

Within each of the functional groups several numbers (parameter 
values) are to be supplied. The numbers are preceded by and 
a description in English. Any line without an is treated 
strictly as a comment line and may be omitted or added without 
effect. The numbers are separated by commas and may or may not 
include decimal points. Scientific notation is not recognized. 
The input file is read and interpreted in subroutine READINP. 
READINP interprets each line according to its outline index 
(i.e., the left-hand line numbers in Figure 3). For example, a 
line with index 2.5 is expected to contain three GPS 
measurement-scheduling parameters (see Figure 3), while line 
3.3.5 is expected to contain an airspeed measurement weight. 
Thus, while the parameter definitions may be changed to suit the 
user, the indices (line numbers) must remain as shown -- unless 
the user wants to modify READINP. 
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Figure 3. Navigation Parameter File, navxxx.INP 
GPS ONCE /SEC WITH EPHEM ERRORS 


1 
1. 
1 . 
1 . 
1 . 
1 . 
1 . 
1 . 
1 . 
1 . 
2 
2 . 
2 . 
2 . 
2 , 
2 
2 
2 
2 
2 


1 

2 

3 

4 

5 

6 

7 

8 
9 

1 

2 

3 

4 

5 

6 

7 

8 
9 


3.3. 

3.4 

3.4. 

3.4. 
3.4, 

3.5 

3.5.1 

3.5.2 

3.5.3 


1 

2 

3 


RUN SCHEDULING 

ALIGNMENT START - 0. 

ALIGNMENT STOP - 0. 

FILTER START * 0. 

STOP TIME - 100. 

NO. OF MONTE CARLO CASES - 5 

START RANDOM NO (500<I<1000) '512 

IC AND IMU ERRORS = 1 

(0-BIAS; 1 -RANDOM) 

MEASUREMENT SCHEDULE - SECS 


BARO ALTIMETER 
RADIO ALTIMETER 
GPS 

DOPPLER 
AIRSPEED 
POSITION FIX 


MSMNT INT-SEC 

START SEC 

STOP SEC 

5. . 

7200. , 

7400. 

1- , 

400. , 

7400. 

1 . , 

0 . , 

7400. , 

10. , 

7200. , 

3000. 

10. , 

7200. , 

7400. 

10. , 

100. , 

7400. 


3 

3.1 

FILTER PARAMETERS 

a priori STATE UNCERTAINTIES (STD. 

DEVS . ) 

3.1.1 

3.2 

PMATRIX (NEGATIVE TO IGNORE 

STATE) 

3.2.1 

3 - POSITIONS - FT. 

= 

300,300,100 

3.2.2 

3 - VELOCITIES -FT/ SEC 

s 

3,3,3 

3.2.3 

3 - TILTS - ARC SEC 


2,2,2 

3.2.4 

3 - GYRO DRIFT - DEG /HR 

= 

.01, .01, .01 

3.2.5 

1 - ACCEL NULL BIAS - MICROG 

= 

10. 

3.2.6 

1 - CLOCK BIAS - NANOSEC 


20 

3.2.7 

1 - CLOCK BIAS RATE NSEC /HR 

■ 

10. 

3.2.8 

3 - DOPPLER ANT. TILT - DEG 

= 

1,1,1 

3.2.9 

1 - AIRSPEED SCALE FACTOR -% 

* 

1 

3.2.10 

2 - WIND ESTIMATE - FT /SEC 

= 

5,5 

3.2.11 

3.3 

3.3.1 

ESTIMATED MEASUREMENT ACCURACY 
BARO ALTIMETER - FT 

. 

50. 

3.3.2 

RADIO ALTIMETER - FT 


5. 

3.3.3 

GPS RANGE - FT 

= 

100. 

3.3.4 

DOPPLER - FT /SEC 

SE 

2. 

3.3.5 

AIRSPEED - FT/SEC 


= 3. 

3.3.6 

POSITION FIX - FT 


= 2. 


FORCING FUNCTIONS 

3 - GYRO F.F. DEG /HR 
ACCEL F.F. MICROG 

FORCING FUNCTION DECAY RATE 
3 - GYRO FF DECAY - SEC 
ACC FF DECAY - SEC 


2 . 0 , 2 . 0 , 2.0 

50. 


= 600. ,600. ,600. 

= 600. 


f 

i 

i 

i. 

i 
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Figure 3 (cont'd) 


4 

4.1 

4.1.1 

4.1.2 

4.1.3 

4.1.4 

4.1.5 

4.1.6 

4.1.7 

4.1.8 

4.1.9 

4.1.10 

4.1.11 
4.2 

4.2.1 

4.2.2 

4.2.3 

4.3 

4.3.1 

4.3.2 

4.3.3 

4.3.4 

4.3.5 

4.3.6 

4.3.7 
5 

5.1 

5.1.1 

5.1.2 

5.1.3 

5.1.4 

5.1.5 

5.1.6 


SIMULATED ERRORS STANDARD DEVIATIONS 
INITIAL ESTIMATION ERRORS 

3 - POSITIONS - FT =0,0,0 

3 - VELOCITIES - FT/SEC = 0,0,0 

3 - TILTS - ARC SEC =0,0,0 

3 - GYRO DRIFT - DEG/HR = 0,0,0 

1 - ACCEL NULL BIAS - MICROG = 0. 

1 - CLOCK BIAS - NANOSEC = 0. 

1 - CLOCK BIAS RATE - NSEC/HR = 0. 

3 - DOPPLER ANTENNA TILT - DEG= 0 . , 0 . , 0 . 

1 - AIRSPEED SCALE FACTOR - % = 0. 

2 - WIND ESTIMATE - FT/SEC = 0..0. 


IMU MODEL ERRORS SF BIAS RANDOM 

GYROS FRAC, DEG/HR = .0000, .0, 0. 

ACCELEROMETERS FRAC.FPS2 = .0000, .0, 0. 


MEASUREMENT ERRORS 

BARO ALTIMETER - FT 
RADIO ALTIMETER FT 
GPS RANGE - FT 
DOPPLER VEL. FT/ SEC 
AIRSPEED - FT/ SEC 
POSITION FIX - FT 


BIAS GAUSS %BAD SCRNLIM MINMSNT 
10. , 50. , 1. , 1. , 

1., 5., 2., 1.5, 500. 

5. , 5. , 0. , 2. , 

1. , 2. , 4. , 2.5 

1., 3., 5., 3., 30. 

10. , 1. , 0. , 3. 


SATELLITE CONFIGURATION 
NOMINAL VALUES 

NO OF ORBITS (MAX 6) 

ORBIT INCLINATION DEG. 

NO OF SATELLITES /ORBIT (MAX 
ASC NODE EACH ORBIT DEG 
ANOMALY OF FIRST SAT DEG 


= 3 
=60. 

6 )= 6 , 6 , 6 , 0 , 0,0 
= 0. , 120. ,240. ,0 
=0. , 20. , 40. ,0. 


, 0 . ,0 

0 . , 0 . 


5.2 SATELLITE SELECTION FOR GPS 

5-2.1 DESIRED GDOP =0.0 

5 -2. 2 INTERVAL OF FIXED SAT SELECTION SEC =600. 

5-2.3 MIN ELEV VIEW ANGLE DEG =5.0 

5.2.4 


5.3 EPHEMERIS ERRORS STANDARD DEVIATION IN FEET 

5 - 3 -l RADIAL CROSS-TRACK IN-TRACK 

5 - 3 -2 = 5., 5., 10. 

5.3.3 

6 GHA AND RUNWAY REFERENCE 

6.1 INITIAL GHA =0.0 

6.2 RUNWAY LAT.LON.AZ =37.5,-121.0,0.0 

7 END OF INPUT DATA 
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Run scheduling and error treatment.- The four times which are to 
he specified Call in seconds) are: 

1 . alignment start time 

2 . alignment stop time 

3. filter start time 

4. run stop time. 

Alignment should coincide with a stationary trajectory period and 
use precise position fixes as measurements. Filter start time is 
a parameter for delaying the start of filtering relative to 
navigation (i.e., TAFCOS) time, which always starts at t-0. 

The error treatment inputs relate to the Monte Carlo 
operation of NAVSIM II. The program will repeat the same case n 
times if n is specified greater than one in line 1.5. The 
program's "random" number sequence is initiated at the m-th 
sample, where m is supplied at compilation and where m is the 
number supplied at input line 1.6. The error flag at line 1.7 
permits the use of specific errors (flag^O) in place of random 
errors (flag=l). Thus, when the flag is zero, the numbers found 
in Group 4 (see Figure 3) are used directly as the simulated 
errors. When the flag is non-zero, the Group 4 numbers are used 
as standard deviations (scales) for the unit -variance "Gaussian" 
pseudo-random errors to be initiated for each Monte Carlo pass. 

Measurement schedule.- The numbers in this schedule represent the 
interval, start-time and stop-time for each measurement type. 

The resolution of the interval is one second, since this is the 
filter's cycle rate. Measurements are omitted or suppressed by 
delaying the "starting" of observations until after the run's 
stop-time . 

Filter parameters.- The first set of numbers in this group 
contains the square roots of the a priori covariances of the 
initial diagonal P-matrix (i.e., D-matrix in the U-D 
formulation). That is, these numbers represent the initial 
uncertainty in the estimation of state. The initial diagonal 
covariance matrix assumes that the initial error states are 
statistically uncorrelated. 

The second set of numbers in the group contains the 
measurement weights (uncertainties) for use in filter-processing 
of the various measurements. In the case of Doppler radar and 
position fixes, the single number specified here is used to 
weight each axis component of measurement. 



The third set of numbers contains the scales for gyro and 
accelerometer process noise. The incorporation of this process 
noise keeps the error covariance matrix (P -matrix, D-D matrix) 
from becoming too optimistic and thereby excluding measurement 
information from the estimate. The fourth set of numbers 
represents the time-constants in the process-noise integration 
(filter) model. 


Simulated errors.- Simulated errors include errors in initial 
estimation of the state elements, IMU model errors and 
measurement errors. These represent the "actual" system errors 
of the simulated aided-inertial navigation system. If the error 
treatment flag (line 1.7) is set to 1, these numbers are used as 
the standard deviations (scales) for pseudo-random, unit-variance 
errors. If the flag is set to zero, the numbers represent the 
specific and particular errors of the system. 

The first set of numbers describes the errors in the initial 
estimate of state. The initial estimate is derived in NAVSIM II 
(in particular, in subroutine INESTM) by subtracting these errors 
from the "actual" initial state. 

The second set of numbers describes the error models for 
gyros and accelerometers of the strapped-down IMU. The scale 
factor, bias and random (Gaussian) errors apply to all three axes 
of these sensors. Scale factor and bias errors are randomly 
sampled as constants for each Monte Carlo pass. The "random" 
value is the scale for inertial sensor noise at every navigation 
integration step (20 times/second). 

The third set of numbers describes the "actual" measurement 
errors for the various measurement types plus screening limits 
for bad measurement data and minimal data values for processing. 
That is, four or five numbers are supplied for each measurement 
type. The first number is a bias scale for random selection of 
measurement bias at each Monte Carlo pass. The second number is 
the scale for Gaussian noise in the measurement. The third 
number is the percentage of "bad" measurements to be simulated. 
These "bad" measurements are simulated by use of a uniform random 
number sequence on the interval (0,1). When the random sample is 
less than the specified percentage, the measurement residual sent 
to the filter is replaced by a large number which should be 
detected by the screening logic. The fourth number is the 
multiple of the weighted residual to be used as a threshold in 
screening bad points from filter processing. The weighted 
residua l is the ratio of the measurement residual to the filter 
weight ,VHPH 1 ’+ R . The fifth number for radar altimeter 
measurements is the limiting altitude for processing -- that is, 
measurements are processed only below 500 feet. The fifth number 
for airspeed measurement represents the lowest airspeed at which 
the data will be processed. 
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GPS satellite information.- This group has three sections 
pertaining to the GPS satellites. The first section pertains to 
the orbital configuration. The first parameter is the number of 
orbital planes and the second parameter is the common inclination 
of these planes to the equator. The next set of numbers 
specifies the number of satellites in each of the orbits. The 
next set of numbers specifies the longitude of the ascending node 
of each orbit, measured in the equator from the vernal equinox. 
The next set of numbers specifies the anomaly (angle) of the 
first satellite in each orbit measured along the orbit from the 
equator. The satellites in any orbit are uniformly distributed 
in angular measure from this first anomaly and remain so 
throughout this simulation under the assumption of circular 
orbits. The orbital period is modifiable only through 
re-compilation . 

The second section of this group pertains to the selection 
and observation of satellites in GPS measurement. Desired GDOP 
is used in selecting four satellites from the 4-8 which may be in 
view. GPS observations cycle through the selected four for a 
specified interval of time (the next entry). A desired value of 
zero causes selection of the optimal set from the point-of-view 
of minimal GDOP. A large value (e.g. , 2000) causes selection of 
the worst set from GDOP considerations. Intermediate values 
(e.g., 6) cause selection of a set whose GDOP is nearest the 
desired value. The final number in this section specifies the 
lowest elevation at which a satellite may be viewed. 

The third section of this group relates to satellite 
ephemeris errors in GPS measurements. The three numbers define 
the scales for random selection of biases in radial, cross-track 
and in-track orbital components. 

Runway reference.- The first number in this group is the 
Greenwich hour angle (GHA) at time-0. This affects only the 
orientation of the satellite orbits with respect to the 
Earth-fixed reference frame. It implies time-of-day and date and 
is equivalent to a bias in the longitudes of the ascending nodes 
of the orbits. The other three numbers are latitude(N), 
longitude(E) and azimuth (CW from N) of the runway reference 
coordinate system. 


16 



Trajectory specification. - An example of the trajectory 
specification file (e.g., TRJXXX.INP) is shown in Figure 4. This 
file is read by TAFCOS to describe the reference flight path and 
initial conditions. The file contains comment lines and 
numerical-data lines. The latter are distinguished from the 
former by the occurrence of " in the latter. Some care must be 
taken to exclude from any comment line. The first 
numerical -data line contains runway-x and runway-y components of 
wind. The second contains the initial waypoint index in the 
waypoint table which comes later. The third and fourth 
numerical -data lines contain initial position and velocity 
offsets relative to the initial waypoint. That is, initial 
position and velocity are taken to be those of the initial 
waypoint plus the offsets. The three numbers represent the x, y 
and 2 (downward) components of the offset. It must be noted that 
these are guidance offsets and not navigation errors. 

The remaining numerical-data lines specify trajectory 
waypoint parameters for TAFCOS. The number of waypoints (lines 
in the table) is limited to 50. The 11 numbers in each line 
represent the values shown in the following table. 


Number in line 

Value 

1 

waypoint index 

2 

x-coordinate of start of straight-line 
path or turn-center 

3 

y-coordinate of start of straight-line 
path or turn-center 

4 

turning radius (zero for straight line) 

5 

initial heading 

6 

approximate path length to be traveled 
to the next waypoint 

7 

speed (airspeed when mode=l, ground speed 
otherwise) 

8 

mode (1 for normal flight, 2 for approach 
and 3 for hover) 

9 

approx, time to traverse the path length 
to the next waypoint 

10 

initial altitude 

11 

flight path angle 


Appendix C contains a discussion of some utility software 
which can aid in preparing a reference trajectory file. The file 
can also be formed directly, by use of the system's editing 
utilities. 



Figure 4. Trajectory Specification File, TRJXXX.INP 


TRAJECTORY DATA FOR LOCAL PATH V IS 101FPS 
WIND VELOCITY FPS = 0.0, 0.0 

INITIAL WAYPOINT * 13 


AIRCRAFT INITIAL CONDITION 


POSITION 

ERROR -X, 

,Y,2- FT 

=0. ,0 

i. .0. 





> 

VELOCITY 

ERROR -X, 

i Y, Z- FPS =0. , 

o 

o 





s 



WAYPOINT TABLE 









X 

Y 

RAD 

INIT 

PATH 

SPEED 

MODE 

TIME 

ALT 

FLT P/»'! 

WP 



TURN 

HDG 

LNGTH 

* 

* 

IN SEG 


ANGI f 

NO 

FT 

FT 

FT 

DEG 

FT 

FPS 


SEC 

FT 

DEG 

= 1 , 

-9000 

0. , 

0. , 

0. , 

0. , 

-5. , 

3. , 

20. , 

500. 

0. , 

=2, 

-9000 

, -3000. , 

-3000. , 

0. , 

9425 . , 

101. , 

1. , 

140. , 

500. 

, 6.0 f 

=3, 

-9000 

,-6000. , 

0. , 

160. , 

7000. , 

135. , 

1. , 

44. , 

1500. 

, 0. ’ 

=4, - 

■16000 

, -3000. , 

-3000. , 

180. , 

9425. , 

135. , 

1. , 

70. , 

1500. 

, 0. 

=5, - 

16000. 

0. , 

0. , 

0. , 

7000. , 

135. , 

1. , 

44. , 

1500. 

, 0. 

“6, 

-9000. 

,-3000. , 

-3000. , 

0. , 

9425. , 

135. , 

1. , 

70. , 

1500. 

, o. l 

=7, 

-9000. 

, -6000. , 

0. , 

180. , 

7000. , 

135. , 

1. , 

44. , 

1500. 

, 0. 

=8 , - 

16000. 

, -3000. , 

-3000. , 

180. , 

9425 . , 

135. , 

1. , 

70. , 

1500. 

, 0. t 

-9, - 

16000. 

0. , 

0. , 

0. , 

7000. , 

101. , 

1. , 

50. , 

1500. 

, 0. i 

-10, 

-9000. 

,-3000. , 

-3000. , 

0. , 

9425 . , 

101. , 

1. . 

93. , 

1500. 

0. 

“11. 

-9000. 

,-6000. , 

0. , 

180. , 

7000. , 

101. , 

1. , 

60. . 

1500. 

0. t 

-12.- 

16000. 

,-3000. , 

-3000. , 

180. , 

9425 . , 

101. , 

1. , 

93. , 

1500. 

0. f 

-13,- 

16000. 

0. , 

0. , 

0. , 

1000. , 

101 . , 

1. , 

10. , 

1500. 

o. ■* 

-14,- 

15000. 

. 2000., 

2000. , 

0. , 

12566. , 

101. , 

1. , 

150. , 

1500. 

, -3.92 

-15,- 

15000. 

0. , 

0. , 

0. , 

6000. , 

5. , 

2. , 

83. , 

637. 

, -1.31 [ 

-16, 

-9000. 

0. , 

0. , 

0. , 

0. , 

-5. , 

3. , 

20. , 

500. 

, o. 1 

* IF 

MODE 

IS 1 NORMAL FLIGHT SPEED IS 

AIRSPEED 




IF 

MODE 

IS 2 APPROACH 

SPEED IS 

GNDSPEED 



l 

IF 

MODE 

IS 3 HOVER 

SPEED IS 

GNDSPEED 
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Output Description 


Printed Output Files 


NAVSIM II produces seven output files in addition to the 
system output file. These files may be displayed by invoking the 
system PRINT or TYPE utilities. The output files are- - 


1 . 

2 . 

3. 

4. 

5. 

6 . 
7 . 


navxxx . OIN 
navxxx. ER1 
navxxx. ER2 
navxxx. SGI 
navxxx . SG2 
navxxx .RES 
navxxx . TR J 


echo of the input files 
estimation errors, part 1 
estimation errors, part 2 
uncertainty in estimation, part 1 
uncertainty in estimation, part 2 
measurement residuals 
trajectory (actual). 


The -.OIN file echoes the -.INP files of navigation and 
trajectory data and also lists the randomized bias-type errors 
which were actually used for each Monte Carlo pass . 


The -.ER1 and -.ER2 files tabulate the estimation errors. 
The sense of these errors is (actual value - estimated value). 


The -.SGI and - . SG2 files tabulate the uncertainties in the 
estimates as indicated by the square roots of the diagonal 
elements of the state covariance (P) matrix. One line of each of 
these files is produced after each time-update and after each 
measurement event . The lines ' sources are identified by an entry 
in a column labeled "DATA TYPE." 


The -.RES file tabulates the measurement residuals which 
were sent to the filter for processing. In the case of GPS 
measurements, the satellite number and current GDOP measure are 
also tabulated. 

The - . TR J file tabulates the true or actual trajectory 
components of position, velocity and specific force. This file 
is written only during the first Monte Carlo pass, since the 
actual trajectory is invariant from pass to pass. 



Plots 


Utility software for NAVSIM II includes a plotting 
capability for graphical display of estimation errors, the 
trajectory and measurement residuals. This capability is invoked 
by typing 


NAVPLT 

at a graphics terminal such as a Tektronix 4014. This causes the 
following menu to be displayed. 

Current plots available 

1 . Error plots 

2. Trajectory plots 

3. Measurement residual plots 

8. DIPTEK (display previously created DIP file) 

9. Exit (return to system '$' level) 

Enter number of plot desired: 

Input file name: 

When the user responds to the first prompt with a 1, 2, 3 or 8 he 
is asked for a file name. The NAVPLT plotting capability reads 
the output files produced by NAVSIM II for data to be plotted. 
Error plots use the -.ER1 and -.SGI files, trajectory plots use 
the - . TR J files and measurement residual plots display data from 
the -.RES files. DIP files are device-independent -plotting files 
which are always created for hard-copy display whenever the user 
responds with a 1, 2 or 3. The DIP files are filed as new 
versions of: 

ERRPLT.DIP for error plots 

TRJYPLT.DIP for trajectory plots 

RESPLT.DIP for residual plots. 

These may be re-displayed later at the terminal using option 8. 

When the user has provided a filename following a 1 , 2 or 3 
response to the initial prompt, he will be asked to furnish 
scaling information. The user is given the opportunity to 
override the automatically scaled values for abscissa and 
ordinate scales. He is prompted with the default values. A 
simple carriage return will retain those values. An entered 
value will override the default. 
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Error plots (Figures 5 and 6) display two plot-pages of 
estimation errors and their uncertainty envelopes. The first 
page depicts position errors versus time and the second depicts 
velocity errors versus time. The major plot title is taken from 
the first 40 characters of the run title. The uncertainties come 
from the -.SGI file while the errors are taken from the -.ER1 
file. 


Trajectory plots (Figure 7) display the horizontal and 
vertical projections of the actual (true) trajectory. The 
vertical plot's abscissa is distance along the horizontal path 
(rather than x or y). The scales for these plots are fixed, but 
may be changed by a procedure described in Appendix C. 

Measurement residuals (Figure 8) are displayed versus time 
as discrete points, rather than being connected by lines. As 
with the position and velocity error plots, the user is given the 
opportunity to define scales to override the data-based automatic 
scales. In the case of GPS range measurements, the range 
residuals for the different satellites are each displayed with 
different plotting symbols. 


Z-ERROR Y-ERROR X-ERROR 


GPS ONCE/SEC WITH EPHEM ERRORS 

POSITION ERRORS 
TIME IN SECONDS 





GPS ONCE/SEC WITH EPHEM ERRORS 

VELOCITY ERRORS 
TIME IN SECONDS 



Figure 6: Velocity Error Plot 
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GPS ONCE/SEC WITH EPHEM ERRORS 

TRAJECTORY PLOTS 


25.0 -20.0 -15.0 -10.0 -5.0 0.0 5.0 1 

X TRACK (FT * 1000) 


D 45.0 60.0 75.0 90.0 105.0 120.0 335.0 150.0 

DISTANCE TRAVELED (FT * 1000) 


Figure 7: Trajectory Plot 










GPS ONCE/SEC WITH EPHEM ERRORS 


MEASUREMENT RESIDUALS - GPS 



TIME IN SECONDS 

Figure 8: Measurement Residual Plot 




Hard-copy plots 


Hard-copy plots may be obtained from tbe DIP files 
(ERRPLT.DIP , TRJYPLT . DIP , RESPLT. DIP) and displayed with tbe help 
of system utilities: 

DIPQMS for laser-printer plots 
DIPVER for Versatec plots. 

To use DIPQMS, the user responds to the system ($) prompt with 
DIPQMS filename 

where filename=ERRPLT.DIP, for example. In this case, a file 
named ERRPLT.QMS will be produced by DIPQMS. This file may be 
printed by entering: 

PRINT / QUEUE = LASER / SETUP= PORTRAIT ERRPLT . QMS 
If the user wishes Versatec plots, he first types: 

DIPVER filename 

to produce the default files PARM.PLV and VECTR1.PLV. He then 
types 


PLOT 


and is prompted for: 
Prompt 


Response 


Node: SAT 

Type : VERSA 

Parameter file: return 

Vector file: return 

Type : return 


(defaults to PARM.PLV) 
(defaults to VECTR1.PLV) 
(ends this dialogue) 


The Versatec plots will then be printed at the specified Decnet 
node, Saturn Vax in this example. 
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Section IV 
PROGRAM DESCRIPTION 


Philosophy of design 

The design philosophy of NAVSIM II was to try to build a 
core navigation simulation program which could be easily used, 
modified, transported and understood. Toward this end, great 
care was taken in program design to decouple the various program 
functions, to modularize logically and to minimize the user's 
workload. Care was also taken in implementation to write source 
code which was easy to read so that modification and debug time 
would be minimized. Program efficiency was considered to be of 
only secondary importance. 


Structure 


Figure 9 is a combination roadmap and high-level structure 
chart of NAVSIM II. It reads left-to-right and top-to-bottom and 
shows both "who calls whom" and in which order. It shows that 
the executive routine calls (in order), READINP, INEREF and 
SEESAT, then calls MONTEC and TAFCOS for each Monte Carlo pass 
and finally calls OUTCLSE to close the output files. The group 
of subroutines emanating from MONTEC is executed only once per 
Monte Carlo pass. FILSIM is called by TAFCOS every integration 
step (every 1/20 second) and drives the aided-navigation 
simulation logic. The navigation logic (integration of the 
equations of motion) is carried out 20 times per second, while 
the filter's cycle time is once per second. Thus NAVEQ and AUPT 
are called 20 times per second while the remaining subordinate 
routines of FILSIM are called only once per second. 

Subroutine MEASIM implements the measurement schedule, 
calling each requested measurement simulation module at the 
scheduled times. The measurement inputs to the filter — 
residuals, sensitivities and weights — are queued up for 
sequential processing by the filter logic (MEASUP, UDMES). The 
filter's three significant operations are time-updating of the 
U-D matrix (in TIMEUP) , measurement updating (in MEASUP) and 
estimate-updating (in CHNGX). The remaining functions are 
supportive of these three operations. The following time-line 
sketch shows the operational sequence. 


n seconds 


n+1 seconds 



) , i 






t 1 1 

M 1 J 

Jl <a * i 

■If | • | II 



non-linear 
estimate update 
and transition 
element summation 



measurement 
queuing 



time 

update 


measurement 
update 


estimate 

updating 
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Data Flow 


Figure 10 is a high-level data flow diagram of NAVSIM II. A 
more detailed data flow picture requires an item-by-item and 
subroutine-by-subroutine exposition. This detail can be found by 
examination of the commons descriptions found later in this 
section. Figure 10 shows the major program groups and the flow 
of data among them. The once-per-run initialization routines 
read the input files and initialize earth-reference variables and 
satellite orbits. The once-per-pass routines re-establish 
initial conditions for the actual trajectory and for the 
estimated trajectory. These are passed to the integration 
routines, TAFCOS and NAVEQ, along with the actual and estimated 
IMU errors. The error state covariance is also reset to its 
initial (input) value and passed to the Kalman filter routines. 
Initial measurement biases are also established by random 
sampling for use by the measurement simulation routines. The 
integration routines supply the actual and estimated position and 
velocity to the measurement routines and compute the state 
transition matrix elements for use by the Kalman filter's 
time-update routines. In return, updates to the estimated state 
are provided to NAVEQ by the Kalman filter routines (esp. 

CHNGX). The measurement simulation routines supply measurement 
residuals, sensitivities and filter weights to the Kalman filter 
routines where they are processed to produce changes in the 
estimates of navigation state (returned to the integration 
routines) and measurement biases (returned to the measurement 
simulation routines). These are the major data flows of NAVSIM 
II. Data flows which are strictly for output purposes are not 
shown. 


->q 
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Figure 10: Data Flow of NAVSIM II 



Subroutines 


The subroutines which comprise NAVSIM II are listed in Table 
1 with a very brief description of each. A more-detailed summary 
of each of the subroutines is given in Appendix D. The summary 
lists parameters set and used by the routine as well as names of 
subroutines the routine-of-interest references and the other 
NAVSIM II routines which reference it. No mathematical 
formulations or algorithms are described; these can be found in 
Appendix A, in the references and in the Fortran source code. 


Commons 

Most inter-module communication in NAVSIM II is by means of 
labeled commons (as opposed to call-lists). These commons have 
been structured from logical consideration of their contents. 
Table 2 lists the commons and gives a very brief description of 
each. A description of each common will be found in Appendix E. 
In addition to a definition of each parameter in the common, the 
description contains a set /used table by parameter and by 
accessing module. 


Files 

NAVSIM II reads two input files and writes seven output 
files in addition to the system output file. The input files are 
read in by modules READINP and TAFCOS. Output files are written 
primarily in module OUTPUT. All input and output files are in 
ASCII format. Table 3 lists these files. 
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Table 1 

Subroutines /Modules 
Module Function 


NAVSIM (main) Driver or executive for NAVSIM II 


AIRSPEED 

ALTMTR 

APFILL 

AUPT 

BARN1 

CHNGX 

CNVLIN 

DOPPLER 

ESTMERR 

FILNIT 

FILSIM 


Formulates and queues up residuals, sensitivities 
and weight for airspeed measurements 

Formulates and queues up residuals, sensitivities 
and weights for barometric and radio altimeter 
measurements 

Fills transition matrix elements for time-update 

Computes and sums state-rate sensitivities to state 
variations for transition matrix computation 

Generates pseudo-random numbers 

Incorporates state-change estimates into the 
estimated state for the extended filter 

Converts ASCII-string numbers into internal 
floating-point numbers 

Formulates and queues up residuals, sensitivities 
and weights for Doppler radar measurements 

Computes the state estimation error 

Initializes the Kalman filter parameters 

Drives the simulation of the airborne Kalman filter 


GDOP 

GPS 

INEREF 

INESTM 

INTRUE 

KALFIL 


Computes GDOP for four satellites 

Formulates and queues up residuals, sensitivities 
and weights for GPS measurements 

Initializes equator-to-runway transformation, 
earth-rate vector and radius of curvature 

Initializes the estimated state for each Monte 
Carlo pass 

Initializes the simulated actual state for each 
Monte Carlo pass 

Controls high-level Kalman filter sequencing logic 
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HE A SIM 

Controls measurement simulation according to input 
schedule 

MEASUP 

Controls processing of one measurement by the 
Kalman filter 

MONTEC 

Controls initialization process at the start of 
each Monte Carlo pass 

NAVEQ 

Integrates navigational equations for one time-step 

OUTPUT 

Performs most of the file-output tasks for NAVSIM II 

PHIX 

Performs linearized time-update (multiplies vector 
by the state transition matrix 

POSFIX 

Formulates and queues up residuals, sensitivities 
and weights for position-fix measurements 

READINP 

Reads, interprets and distributes data from the 
navigation-parameter input file 

SEESAT 

Determines GPS coverage (satellite visibility) and 
optimal GDOP 

TAFCOS 

Generates helicopter's "actual" trajectory, forces, 
body rates and attitude 

TIMEUP 

Controls time-updating of the covariance matrix 
and incremental state for the Kalman filter 

TRIMAT 

Displays the U-D covariance matrix in 
upper-triangular format 

TRUSAT 

Initializes the "actual" GPS satellite ephemerides 
and errors 

UDMES 

Updates the U-D matrix and incremental-state 
estimate for one measurement 

UD2SIG 

Computes standard deviations (state uncertainties) 
from a U-D matrix 

WGS 

Reduces an input rectangular matrix to U-D 
(upper-triangular) form 
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Table 2. List of Commons 


ACTUAL 

AUPCOM 

CONST 

DELCOM 

EROTIC 

ERRCOM 

GRVCOM 

KALM 

KALMIN 

MESCOM 

ME SEED 

MESWTS 

ORBITS 

OUTCOM 

PRTCOM 

REFCOM 

STATCM 

TIMCOM 

TIMEIN 

TRANS 

TRUORB 

TRUTHC 

VISCOM 


- Simulated truths in sensor models 

- Time-constants for inertial-sensor process noise 

- Global constants (radians-to-degrees , etc.) 

- Incremental state estimates 

- Earth's rotational rate and radius vectors 

- Error state initial condition and sensor errors 

- Estimated gravity vector 

- Kalman filter covariance parameters 

- Kalman filter input parameters 

- Measurement processing parameters 

- Measurement scheduling parameters 

- Measurement weights and errors 

- Orbital data for GPS satellites 

- Output file descriptors 

- Extra-output print option flags 

- Earth-referenced location/frame variables 

- Current estimates of state 

- Time indicators 

- Case-control times 

- Transition-matrix parameters 

- True GPS orbital parameters 

- simulated true aircraft state 

- GPS visibility parameters 




Table 3. 

Input /output 

Files of NAVSIM II 


Logical 

Unit 

File 

Cont ent s / purpose 

Input 

1 

TRJXXX . INP 

reference trajectory specification 


4 

navxxx . INP 

navigation parameter inputs 


5 

System inp. 

Input filenames from user 

Output 

6 

System out. 

miscellaneous & debug print 


7 

navxxx . TR J 

tabulated trajectory coords. 


8 

navxxx . RES 

measurement residuals 


9 

navxxx. ER1 

estimation errors, part 1 


10 

navxxx . ER2 

estimation errors, part 2 


11 

navxxx . SGI 

estimation uncertainty, part 1 


12 

navxxx . SG2 

estimation uncertainty, part 2 


13 

navxxx . OIN 

echo of the inputs Cnav + trj) 
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Section V 
RECOMMENDATIONS 


NAVSIM II is a tool which can aid researchers in 
understanding and predicting the effects of various system 
parameters and errors on performance of an aided-inertial 
navigation system for helicopter flight. As any tool, it must be 
used to be of any value. Its use will undoubtedly lead to its 
modification as enhancements and extensions to its original 
capabilities are seen to be needed. It was with the likelihood 
of modification in mind that NAVSIM II was designed with 
attention to structure. 


Enhancements 

A number of useful enhancements to NAVSIM II can be 
suggested, even at the program's debut. But for time 
limitations, many of these would already be included among NAVSIM 
II 's capabilities and features. The order of the following 
suggested enhancements does not imply importance or priority. 

1. Save and re-start from a final covariance matrix. 

This would enable shorter runs which concentrate 

on the terminal or approach phase while representing 
correlated uncertainties from take-off and cruise 
phases. The saved covariance matrix could also be 
used as the source of Monte Carlo initial condition 
errors for the terminal phase. 

2. Enable variation of the filter's cycle time and 
estimation-update time. This would allow longer 
Kalman cycles, thereby reducing the computational 
load and enabling investigation into corresponding 
dependency on assumptions of linearity. Delay in 
updating the estimate with estimated state increments 
is common in airborne implementations. Such delay 
may tax linearity assumptions and usually generates 
special requirements in calculation of partials and 
residuals. Increasing the filter's cycle time will 
generally require an increase in the size of the 
measurement-processing queue. 

3 Permit a skip-rate for print-files. This would 
reduce the size of the output files produced by 
NAVSIM II at the expense of detail and some 
additional complexity in the plotting software. 

4. Provide flexible plotting to select data from 

arbitrary columns of output from several specified 
runs. This capability would permit display of the 
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effects of parametric differences — not just Monte 
Carlo variation. This enhancement could be extended 
to include specified mathematical operations on the 
output columns of numbers before plotting — thus 
enabling such displays as translated and rotated 
coordinates, mean-square errors, metric units. 

5. Provide the square-root and conventional P-matrix 
implementations as options to the U-D filter 
formulation. This should make no difference in 
the results, but should help to satisfy any 
questions of the method. 

6. Provide more flexibility in specifying filter 
weights and process noise. This flexibility 
would permit examination of ad hoc methods for 
tuning the filter, such as scheduled variations 
in the level of process noise (random forcing 
functions). This capability, along with more 
flexibility in scheduling of the various 
measurements, would enhance the simulations of 
INS alignment and pseudo-differential GPS. 

7. Provide an option for simulation of a stable-platform 
type of INS . 

8 . Enable Runge-Kutta integration of the navigation 
equations. This would enable the use of larger 
integration steps and might reduce run-time. 

9. Provide an interactive program for producing and/or 
editing the navigation-parameter input file through 
prompts . 

10. Improve the means of specifying the reference 
trajectory. The utility provided with NAVSIM II 

is helpful in this regard, but is still hard to use. 

11. Extend the measurement set to include GPS 
delta-range (range rate). 
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Extensions 


Several logical extensions to NAVSIM II are envisioned. 
Extensions differ from enhancements in that they make a 
significant change in the purpose or use of the program rather 
than simply enhancing its current function. 

1. Extension to guidance. This extension would feed 
navigation errors back into the guidance calculations 
(TAFCOS), thereby potentially simulating guidance 
errors which include navigation errors. The resulting 
program would be primarily a guidance research tool. 

2. Flight test analysis. This extension would use 
flight-recorded data in a filter-smoother implementation 
to examine the effect of specifiable parameters on 

the resultant navigational estimate. This extension 
would require a major modification of the program. 
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Appendix A 
MATHEMATICAL BASIS 


Equations of Motion and Their Errors 


The equations of motion (i.e., navigational equations) of the 
aircraft are (from reference [3]): 


v = f + g - (2u e + w) x v (A. 1) 

x = v (A. 2) 


Where 


f is the specific force vector. 

g is the gravity (reaction) vector 

oj is the earth-rate vector 
e 

u> is the rotation rate of the computational reference 
frame with respect to the earth 

v is the velocity vector relative to the reference 
frame's origin 

x is the position vector relative to the reference 
frame's origin 


The computational reference frame is runway - referenced, centered 
at some point on the runway with x-axis positive in the normal takeoff 
and landing direction, y-axis to the right and z positive downward. 

The vectors must be transformed to common reference frames before 
the equations of motion can be formed and integrated. Let a lower-left 
subscript denote the reference frame in which a vector is coordinatized. 
For example. 


^f is the specific force vector expressed in body-frame 
components. 

f is the specific force expressed in runway-frame components. 
The transformation from body to runway coordinates is denoted r T^’ 
Thus r f - b f 


A-l 



1 


Further, let the "true" values of a variable be denoted with a bar 
over the variable's symbol and let the "estimated" value be denoted 
with a caret. 

f true specific force 

g estimated gravity 

The implementation of equations A.l and A. 2 (in subroutine NAVEQ) is 
as follows. 

1. Compute measured specific force 


, f =,f. s , + , f, . + , f 

b meas b f b bias b noise 


(A. 3) 


Compute corrected specific force in runway coordinates 
,b 


f = T , f 
r corr r b meas 


0 

0 


r b 


(A. 4) 


3. Compute transformation from navigation-frame (tilted) to 
runway frame, using estimated tilts , ft 


r T n = ) - i - ( 0 * ) 


(A. 5) 


4. Compute acceleration in the navigation frame by adding gravi- 
tational and coriolis contributions to the corrected specific 
force 


a = T f +g+c 

n nr corr n 6 n 


(A. 6) 


5. Integrate acceleration to render velocity 

n v (t) = n v (t-6) + n a *$ (4 = integration step) (A. 7) 

6. Compute average velocity at mid-integration-step 


n v avg = ( n v(t) + n v ( t - 6 »/2 


(A. 8) 


A- 2 



7. Integrate velocity to render position 


r x(t) = r x(t-<5) + r T 


,n 


v 

n avg 


8. Compute measured body rate vector 

b w meas = b W * S u> + b“bias + b^noise 

9. Compute corrected body rate vector 

a 

^u(t) = b^meas b ^bias 

10. Compute average rotation vector 
0 = .5 • ^u(t) + ^u(t-6)j ' 5 

11. Update the estimated body-to-runway transformation 
b T r (t+6) = ^(0) b T r (t) 

12. Update gravitational acceleration 


(A. 9) 


(A. 10) 


(A. 11) 


(A. 12) 


(A. 13) 


,g(t+S) = (l+2x 3 /r ) T r 


e n 


- * 2 /r. 


Vl-^/r) 2 ~ (x 2 /r ) 2 


(A. 14) 


13. Update coriolis acceleration 

„r 


n c (t-rt) = n v X |2 n T t r 0) e + 3 X r £ 0 e + r T b ( b w blafl - b w bi as)J (A.15) 


A-3 


(A. 16) 


14 . 


15. 


Update the drift-rate estimate 

a. ^ b 

io, * - io x 6 + T ( to. . - ,io, 

r d r e r r b bias b bias 

Update the tilt estimate 

6(t+6) = 6(t) + ( r io d x 6/2 + r io d >*6 


) 


(A. 17) 


Estimation errors propagate through the equations of motion because 
these non-linear differential equations are functionally dependent 
on the errors in the estimate. Additional errors develop in the 
estimate because some IMU errors are included in the error model 
("true" errors) but are not included in the estimated state. 
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Variational Equations 


The error covariance matrix is time-updated linearly, using 
the state transition matrix and forcing function sensitivity 
matrix in the update. These propagation matrices are derived 
from solutions to appropriate variational equations. 


The variational equations for NAVSIM II are developed in 
Reference [ 3 ] . They are conveniently summarized using vector- 
matrix symbology. If we let z represent the error vector, its 
time-variation is approximately by 


z = F z + F n 
z n 

where n is the process-noise vector and where F and F are 
r z n 

matrices. 


F 

z 


iL . 

3z 


F * 
n 


ii 

3n 


Equation (A. 2.1) has the solution 

z(t) = $(t) z(0) + $ (t) n(0) 

n 

where $ is the state transition matrix and $ is the forcing- 

n 

function sensitivity matrix. These obey the following differential 
equations . 


$ = F $ with $ (0) = I 
z 


$ = F $ + F 

n z n n 


V°> = 


A- 1 


(A. 18) 


(A. 19) 


(A. 20) 


with 


0 


(A. 21) 



These differential equations are not solved explicitly in NAVSIM II. 
Alternatively, the assumption is made that 


20 

4=1 +23 F z (x(i6)) -6 
i*l 


4 = F (A)*A 

n n 

where 5 is the integration step and A is the filter-update interval. 
The product 4 of equation A. 2. 2 is formed in subroutine PHIX, taking 
advantage of the known sparseness of 4 in formation of the product. 


The error state vector, z, for NAVSIM II has 21 elements: 


1-3- 

X 

position error vector, feet, runway coordinates 

4-6* 

n V 

velocity error vector, ft /sec, navigation-frame 


11 

coordinates 

7-9- 

8 

tilt error vector, radians, runway coordinates 

10-12. 

“d 

drift rate error vector, rad/sec, runway coordinates 

13. 

f b 

vertical acceleration bias, ft /sec/sec, runway 



coordinates 

14. 

c b 

GPS clock bias error, nanosec 

15. 

*b 

GPS clock bias rate error, nanosec/second 

16-18. 

“d 

Doppler radar antenna pointing error vector, degrees 

19. 

S / 
a/s 

Airspeed scale factor error, dimensionless 

20-21. 

w 

V 

wind speed error vector, ft /sec, runway coordinates 


(level only) 



The partial derivative, T^, of z with respect to z is developed in 
reference [3], It can be written as a matrix. 


0 

F 

XV 

F xg 

0 

0 

0 

1 






F 

vx 

F 

W 

F vg 

F vu 

F vf 

0 

0 

0 

F B8 

F 6u 

0 

0 

0 

0 

0 

F 

okjj 

0 

0 

0 

0 

0 

0 

F ff 

0 







0 

0 

0 

0 

0 

D 


The non-zero elements of are all 3x3 matrices except for 
(a scalar) , F vf (3 x 1) and D (8 x 8). They are implemented in 
subroutine AUPT. 


F 

xv 


I 


(3x3 identity matrix) 


3 

i 

i 


j 



(cross-product matrix) 


F = = function of g and x (see ref. [3]) 

vx 3x 


=-2 to x 
\r\r t & 


F 


(cross-product matrix) 



TO 

II 

- r 8 > 


A 

F 

V X 

voo 

n 

II 

(0 0 

= 

- w 

bb 

c e 


*b 

F- = 

T 

Bco 

n 

F 

_ 1 

coco 

T 


10 

F =» 

1 r r 

1 


n 


r e 


ff 


x) 


D *= Ig + off-diagonal term for clock bias wrt clock bias rate 
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Filter Equations 


The conventional Kalman filter equations are: 


Time-update: P + = 

Measurement update: 


T T 

$p $ + a wr 

T n n 

A A A 

z + = z + K (y - Hz ) 
with K = P + H T (HP + H T +R) 
and P = (I-KH)P 

t "T T 


-1 


(A. 22) 
(A. 23) 


(A. 24) 


In these equations, 

$ is the state transition matrix 
$ is the process-noise sensitivity matrix 
P is the error state covariance matrix 
W is the process noise covariance matrix 
z is the estimated error state 
y is the measurement residual value 

H is the sensitivity (vector) of measurement to state variation 
R is the covariance of measurement uncertainty. 


At time-updates, P is updated (from P to P + ) by equation A. 22. At 
each measurement update, the error state estimate is updated (from z_ 
to Zj) by equation A. 23 and P is modified (from P + to P | | ) by equation 
A. 24. Then P | | is used in place of P + for the next measurement and P + 
becomes P for the next time update. 

In the U-D formulation, U is an upper-triangular matrix and D 
is a diagonal matrix such that at any time 

P = UDU t 

The details of the time and measurement update processes are to be 
found in reference [1] . 
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Measurement Models and Errors 


I 


The 

measurements modeled in NAVSIM II are 


1. 

baro-altitude (ALTMTR) 

I 

2. 

radio altitude (ALTMTR) 

1. 

3. 

GPS pseudo-range (GPS) 


4. 

Doppler radar velocity (DOPPLER) 


5. 

airspeed (AIRSPEED) 

1 

6. 

position fix (POSFIX) 

f 


The filter sees each of these measurements generically as: I 

i 

I 

1. a measurement residual, y 

2. a measurement sensitivity vector, H, and 

3. a measurement uncertainty (weight), r 


In the following measurement-model summaries, x is a three- 
component position vector, v is a three-component velocity-vector. 
Barred variables (x) represent "true" values, while careted variables 
(x) represent estimated values. The letter b will denote "bias" and 
n will denote "noise" (gaussian) . 


Altimeter Measurement 

This measurement is height above mean sea level for the baro- 
metric altimeter and height above the local terrain (defaulted to be 
sea level) for the radio altimeter. 


f 

I 


I 

I 


T 


h = -x(3) for baro-altimeter 

h = x(3)-h . for radio-altimeter 

terrain 

residual, y = (h + b + n)-h 
sensitivity, H = (0, 0, -1, 0, ..., 0) 
weight , r = input value 


t 


« 


I 
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GPS Measurement 


This measurement is slant range distance from the aircraft's 
position to the observed satellite's concurrent position. The 
satellite-to-be-observed varies at each observation, with the choice 
cycling among a set of four satellites which optimize the GDOP cri- 
terion. Let R s represent the observed satellite's position in the 
runway coordinate frame. Then 

slant range, s = | R -x|=|s| 

s 

A A 

residual, y = (s+e+c+b+n) - (s+c) 

sensitivity, H = (-S(l) /s,-S(2) /s,-S(3) /s, 0,0, . . . ,c^ ,0 0) 


weight, r = input value. 


In the residual equation, e is the observed satellite's ephemeris 
error vector (treated as a bias in the orbital frame and transformed 
to runway coordinates. The letter c denotes the clock bias of the 
GPS receiver, treated as a range bias. In the sensitivity equation, 
the first three terms are runway-coordinate components of the norma- 
lized estimated slant-range vector and c^ is the speed of light 
(sensitivity of slant-range error to clock bias) . 

The residual is not implemented as shown above, but rather in 
a form which does not involve differencing large numbers to render 
a small result. The implementation shown above is written for better 
understanding of the essential computation. 
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Doppler Measurement 


These measurements are modeled as the three components of ground 
velocity along each of the three antenna axes. These axes are assumed 
to differ from the aircraft's body axes by only the small rotation 
which represents antenna misalignment. The body-axis-component vector 
of ground velocity is written in terms of the runway-referenced velocity: 


where ^T r is the transformation from runway to body. The "true” antenna 
misalignment is assumed to be zero, while the estimated misalignment is 
in error by the estimate. It follows that the true and estimated antenna- 
axis components of ground speed are: 


* A * * 

v =(I + ax), T v 
a b r 

residual for axis i, y = ( v(i) + b(i) + n(i))- v(i) 

a a 


sensitivity for axis i, 

H * (0, 0, 0, t n , t i2 , t i3 , 0 vx ir vx i2 , vx i3 , 0, ...0) 


weight, r = input value 


In the preceding sensitivity equation, t . ^ , t 2 and t^ 3 are elements of 
the i-th row of bT r , the runway-to-body transformation. The vs are 
the i-th row elements of the skew-symmetric "v-cross" .matrix: 


0 

-v(3) 

v(2) 

v(3) 

0 

-v(l) 

-v(2) 

v(l) 

0 . 


This matrix is the sensitivity of the three-measurement set with respect 
to antenna misalignments. 
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Airspeed Measurement 


This measurement is the speed of the aircraft with respect to 
the local air mass. Winds are assumed to be horizontal, so the air 
velocity (vector) is 


V 


air 


V xwind 

v(2) - Vina 

v(3) 


and the airspeed is its magnitude. 



The measurement is modeled with a scale factor offset, s, as well as 
simulated bias and noise errors. 

residual, y = ((1 + s)* v a ^ r +b+n) - (1 + s)* v adr 

sen sit ivi ty , H * (0,0,0,h^,h^, h^ ,0,. • . , h^^ , q , h2 ^ ) 

weight, r = input value. 


In the sensitivity equation, the velocity sensitivities are 


(1 + s)*(v(l) - v xwind )/v air 


h 5 “ (1 + s) * (v(2) - V ywind^ /v air 


h = (1 + s)* v(3)/v . . 

o air 

The sensitivity to airspeed scale factor variation is 



and the sensitivities to wind velocity estimates are 
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Position-fix Measurement 


These measurements are modeled as observations of the current 
"true" cartesian position components in the runway coordinate system. 

x-fix residual, y = (x(l) + b(l) + n(l)) - x(l) 

y-fix residual, y = (x(2) + b(2) + n(2)) - x(2) 

x-fix sensitivity, H = (1, 0, ..., 0) 
y-fix sensitivity, H = (0, 1, 0, ..., 0) 
weight, r = input value. 

Position-fix errors in x and y are treated as uncorrelated in the 
current NAVSIM II implementation. 

Bad Measurement Treatment 

NAVSIM II includes a screening feature for excluding "bad" 
measurements (outliers) from incorporation by the Kalman estimator. 

This feature is not necessary in a simulation where the "perfect" 
measurements are corrupted only by biases and white noise, but has 
been included to simulate a "real" system. The criterion for excluding 

a measurement is that the absolute value of the residual (i.e., innova- 

tion) exceed a user-specified multiple, n, of the current measurement 


weight : 

I 

| y | >n* Vhph T + r 

> 


In order to exercise this feature, NAVSIM II simulates a specified 
percentage of "bad" measurements for each measurement type. The 
percentage is used as a threshold for random sampling from a uniform 
distribution covering 100%. That is, 

if random sample < input percentage, then exclude. 

Exclusion is implemented by setting the simulated measurement residual 
to 


= \f~2 * n * r. 
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Appendix B 
PROGRAM MODIFICATION 


NAVSIM II Mas been designed, structured and documented under 
the assumption that it will serve as the basis for extended 
simulations , and that in that role it will inevitably be 
modified. This appendix addresses the elements of such 
modification and points out the parts of this report which can 
simplify the effort. Two likely cases are considered: 

1. a new measurement type is to be added, or 

2. a new measurement -bias-type state is to be added. 

Any modification to the program involves editing and 
compiling the appropriate modules, then linking the new or 
modified object module (s) with the remaining modules to form the 
new executable program. Each of the subroutines of NAVSIM II is 
contained in a separate module with the same name as the 
subroutine C+'.FOR'). Only modules AUPT.FOR, NAVEQ. FOR and 
OUTPUT. FOR contain multiple entry points. The link-module is a 
command procedure called LNAVSIM.COM. The current program is 
linked by 


©LNAVSIM 

The link-module attaches an object library, UTILITY. OLB, to 
satisfy several generic references: 

DOT vector dot-product 

CROSS vector cross-product 

VNORM vector normalization 

MVTRN matrix-vector multiplication 
^?IRN generate a series of canonical rotations 

TWIST generate a rotation matrix from vector rotation. 

If the modification requires a new module, then LNAVSIM.COM 
must be edited to include mention of the new module. It is 
suggested that a "working" version of the program be saved (ie., 
copied or renamed) until the new version is well checked out. 

The following discussion references figures, 
file-conventions and terminology described elsewhere in this 
report. In addition, it is assumed that the reader has access to 
Fortran listings of the modules mentioned herein. 


B-l 



Case 1 : New Measurement Type 


Suppose one wished to add a new measurement (e.g. , range 
to a fixed transponder) to the list of currently available 
measurements. He would then: 

1 . formulate the measurement residual and sensitivities 
in a module called by MEASIM, 

2 . provide for input of related parameters and measurement 
schedules in the input (*.INP) file, commons and in 
subroutine READINP , 

3. provide for output of the measurement residual in 
OUTRE S (in module OUTPUT. FOR). 

Let us call the new measurement module TRANGE (for "transponder 
range") for example. The "true" measurement might be modeled as 

true meas = (1 + s) * ^ (x-x T ) 1 + (y-y^) 2- + + bias + random 

The estimated range might be 

est . range = y , (x-x T ) a + (y-y^.) 1 - 1 - (z-z r ) 2 ’ + bias 

The measurement residual would be modeled in TRANGE as 

residual = true meas. - est. range 

and queued up for filter-processing in YRES(MINDEX) , where 
MINDEX is the measurement -queue index, incremented on entry 
into TRANGE. The sensitivity of the estimated range to the 
filter state variables is 

^r/<^(x,y,z) = (x y z) / r 

where 

[77 Ta 77 7i 77 7a. 

t = yCx-x,.) + (y-y T ) + (z-z T ) 

and 

^r/^bias = 1 (assuming range bias is a filter state) 
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The generally-sparse sensitivity array, H, is upper-loaded 
with the potentially non-zero elements of measurement sensitivity 
with respect to state. These elements are then related to the 
appropriate state-vector element by the indexing array, M. 


E( 1 , MINDEX) - x / r 
H(2 , MINDEX) - y / r 
H( 3, MINDEX) - 2 / r 
H(4, MINDEX) = 1 


M(l, MINDEX) = 1 
M( 2, MINDEX) = 2 
M( 3, MINDEX) = 3 
M(4, MINDEX) = 22 
M( 5, MINDEX) = 0 


(sens, to state 1, x) 

(sens, to state 2, y) 

(sens, to state 3, z) 

(sens, to state 22, bias) 

(no more non-zero sens.) 


The measurement type , weight and time must also be set . 

MTYPE( MINDEX) =10 
SIGMES (MINDEX) = SIGM(IO) 

MSTIM( MINDEX) = MTIME 


The following commons are used for input and output communication 
to TRANGE. 

/ACTUAL/ modified to carry true transponder location 
coordinates and true scale factor 


/MESCOM/ carries queued measurement processing data from 

TRANGE to the filter — modified to change dimensions 
when measurement types grow to exceed 10 


/MESWTS/ provides actual bias and scale for gaussian errors 
plus measurement weights and bad-measurement 
simulation parameters 


/ STATCM/' provides estimated position, estimated transponder 
**■ location, estimated range bias for use in computing 

the residual — modified to add range bias estimate 
and estimated transponder location 


/TIMCOM/ provides time of measurement for storing in measurement 
queue for display purposes 

/TRUTHC/ provides true position for formulating the true 
measurement . 
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Each of the commons is provided as a separate module 
( "name " . CMN) which is incorporated into the various subroutines 
via an "INCLUDE" statement. Each commons module contains a 
comment .line listing the accessing subroutines. This line tells 
which subroutines must be recompiled when the common is changed. 
The modifier must remember to modify this line to include TRANGE, 
for example, for each accessed commons module. The commons 
descriptions files (commons . DOC) , contained in Appendix E of this 
report , must also be updated to include new accessing routines 
(e.g., TRANGE), set /used information and descriptions of any new 
elements added to the common. 

Having the measurement subroutine and commons formulated and 
modified, one would next modify MEASIM to reference the 
measurement subroutine (i.e., TRANGE), following the procedures 
used in calling the other measurement subroutines. 

Module OUTPUT must be modified at entries OUTRES, OUTSIG and 
OUTSIGT to take the new measurement type into consideration. The 
measurement -type labels array, CSIGTYP, must be re-dimensioned to 
11 and "TRNG" inserted as the 10th measurement type. TIME then 
becomes the 11th type by changing IDTYP in entry OUTSIGT. The 
format in OUTRES will have to be changed to tabulate the new 
range residual -- at least as far as the column headings are 
concerned. Transponder range can simply replace the vertical 
component of position-fix measurement at this time. Changes in 
output formats may affect the ability of the plotting software to 
read the files as plotting input. 

The input file, navxxx.INP, and subroutine READINP will have 
to be modified to accept and treat the new measurement 
parameters. Group 2 inputs (see Figure 3) would be extended to 
input a measurement schedule for transponder range. Group 3 
would be extended to include a measurement accuracy (uncertainty) 
for transponder range. Group 4 would be extended to include 
specifications for the bias and random error scales and 
bad-measurement parameters for the new measurement . 

Specifications for the scale-factor error and the transponder 
location(s) and location errors would require a new category in 
the navxxx.INP list -- perhaps logically inserted as group 4.4. 
Subroutine READINP would be modified to (1) read in the new 
parameters and (2) to transfer them to the appropriate commons. 

The measurement errors and transponder locations are 
actuated in subroutines INTRUE (where the "actual" errors and 
transponder locations are actuated for each Monte Carlo pass) and 
INESTM (where the estimated parameters are established at each 
Monte Carlo pass). 

In each case that a subroutine or common is modified, the 
programmer should be sure to document the modification and to 
extend the module's prologue to include the modification. 



Case 2: Addition of a new state 


Suppose one wished to extend the list of states to include a 
transponder range bias for the new measurement just discussed. 

This task is simpler than if the new state were a "dynamic" state 
affecting the equations of motion. The dimensions of some arrays 
must be increased as must the range of some DO-loops. The 
subroutines affected are: 

APFILL - the number of potentially non-zero first-order 

transition matrix elements increases from 73 to 74, 

AUPT - the NRAP (number of elements in a row of AP) array 

in /TRANS/ is extended to 22 elements, the 22nd 
being 1 (one element in the new row, on the diagonal) 

- the NBEG (index of the element which begins each row) 
array in /TRANS/ is extended to 22 elements, the 
22nd being 74 for the 74th packed-array element 

CHNGX - the range bias estimate is added to /STATCM/ and 
incremented by extended DELX(22) of /DELCOM/ 

ESTMERR - the range bias error in /ESTERR/ is computed as the 
difference between the actual bias in /ACTUAL/ and 
the estimated bias in /STATCM/ 


FILNIT - the dimensions of the covariance arrays in /KALM/ 
and /KALMIN/ are changed, as well as dimension- 
constants . 


add 


UD(253) 

GAMMA (21,6) 
PDIAG0(21) 

NST « 21 
IMAXWM - 21 
LENUD = 231 
UD( 253 ) = PDIAGO( 


-«> UD( 276) 

— > GAMMA (22 , 6) 
==> PDIAG0(22) 
==> NST = 22 
=- > IMAXWM - 22 
— > LENUD = 253 
22 ) * * 2 


INESTM - ERRSTATEIC(21) ==> ERRSTATEIC( 22) in /ERRCOM/ 


- estimated range bias is defined as the difference 
between the actual bias from /ACTUAL/ and the error 
from /ERRCOM/ 


INTRUE - actual range bias is initiated at each Monte Carlo 
pass and communicated via /ACTUAL/ 

MEASUP - internal array-dimensions are increased from 22 to 23 
as are /MESCOM/ arrays H, MH and M 

OUTPUT - unit 12 output in OUTSIG entry is changed and SIG 
array is re-dimensioned to 22 
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PHIX 


TIMEUP 


internal array dimensions are increased to 22 
internal array UDI is re-dimensioned to 22. 


The new state requires two input values in navxxx.INP — an 
a priori uncertainty (outline index 3.2.11) and initial 
estimation error (outline index 4.1.11). These must be 
accommodated in READINP, the former being stored into PDIAG0(22) 
and the latter into ERRSTATEIC(22) . 

Each of the affected commons (*.CMN) must be changed and 
their descriptive files (*.DOC) changed as well. Then all 
subroutines which call any of the modified commons must be 
re-compiled before linking. Of course, the affected subroutine 
prologues must be modified to reflect the change. 



Appendix C 
UTILITY PROGRAMS 


This appendix contains information on the utility programs 
used in conjunction with NAVSIM II: the plotting utility, 

NAVPLT, and the trajectory reference utility, QNEW . 


Plotting Utility 

NAVPLT is a Fortran-based data display program which runs 
from the command procedure shown in Figure Cl . It uses tabular 
data files produced by NAVSIM II as its source of data. NAVPLT 's 
program options and capabilities include •• 

1 . displaying 

a. position and velocity errors versus time 

b. ground track and altitude versus position 

c. measurement residuals versus time, 

2. re-display of previously generated Versatec DIP files 

3. automatic or prescribed scaling. 

NAVPLT is written to be run interactively, giving the user 
the opportunity to specify data source files and to control the 
plot scales. It always creates DIP files, however, so that the 
us^^can obtain hard-copy plots as well. Procedures for running 
the program are found in Section III. 
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I 

Figure Cl : RNAVPLT Command Procedure 


$( ******** main driver FOR NAVSIM PLOTS ******* ¥ 

$ ! ' 

$ DIRECT = F$DIRECTORY( ) 

$ SET DEFAULT 'DIRECT' p 

$ TYPE SYS $ INPUT f 

1 * 


f 

\ 


i 

******************************************* 

* Welcome to NAVSIM data plotting program * I 

******************************************* i 

$ TOP: 

$ TYPE SYS S INPUT 


I 

Current plots available : 


1 = ERROR PLOTS 

2 - TRAJECTORY PLOTS 

3 = MEASUREMENT RESIDUAL PLOTS 

8 = DIPTEK (DISPLAY PREVIOUSLY CREATED DIP FILE) 

9 = EXIT (RETURN TO SYSTEM '$' LEVEL) 


l 


f 

i 




$ INQUIRE IDO "ENTER NUMBER OF PLOT DESIRED" ' 

$ IF IDO .EQ. 1 THEN GOTO ERRPLTS * 

$ IF IDO .EQ. 2 THEN GOTO TRJYPLTS 

$ IF IDO .EQ. 3 THEN GOTO RESPLTS \ 

$ IF IDO .EQ. 8 THEN GOTO DIPTEK 9 

$ IF IDO .EQ. 9 THEN GOTO EXIT 

$ WRITE SYSSOUTPUT "" 


$ WRITE SYSSOUTPUT " * * * ILLEGAL INPUT *** " , IDO 

$ GOTO TOP 
$ ! 


A 
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Figure Cl: RNAVPLT (continued) 


1 

! 


T 

1 


» 

i 


\ 

I 

I 

I- 


i 

I 

4 

I 

i 


i 


$ ERRPLTS: 

$ FILE :« 'DIRECT' "IDO. INP" 

$ OPEN /WRITE INPT ' FILE ' 

$ WRITE INPT IDO 
$ CLOSE INPT 
$ ASSIGN 'FILE' FOR007 
$ ASSIGN SYSSCOMMAND FOR005 
$ R FSNO : [ FSNAMA . QPLOT ] RERRPLT 
$ DEASSIGN FOR005 
$ DEASSIGN FOR007 
$ DEL IDO. INP;* 

$ WRITE SYSSOUTPUT "~X" 

$ GOTO TOP 
$ ! 

$ TRJYPLTS : 

$ FILE := 'DIRECT' "IDO. INP" 

$ OPEN/WRITE INPT 'FILE' 

$ WRITE INPT IDO 
$ CLOSE INPT 
$ ASSIGN 'FILE' FOR007 
$ ASSIGN SYSSCOMMAND FOR005 
$ R FSNO : [ FSNAMA . QPLOT ] RTRJYPLT 
$ DEASSIGN FOR005 
$ DEASSIGN FOR007 
$ DEL IDO. INP;* 

$ WRITE SYS$OUTPUT "“X" 

$ GOTO TOP 
$ ! 

$ RESPLTS : 

$ FILE :« 'DIRECT' "IDO. INP" 

$ OPEN/WRITE INPT 'FILE' 

$ WRITE INPT IDO 
$ CLOSE INPT 
$ ASSIGN 'FILE' FOR007 
$ ASSIGN SYSSCOMMAND F0R005 
$ R FSNO: [FSNAMA. QPLOT]RRESPLT 
$ DEASSIGN FOR005 
$ DEASSIGN FOR007 
$ DEL IDO . INP ; * 

$ WRITE SYS$OUTPUT " /> X" 

$ GOTO TOP 
$ ! 

$ DXPTEK* 

$ ASSIGN SYSSCOMMAND FOR005 
$ R FSNO: [FSNAMA. QPLOTlDIPTEK 
$ DEASSIGN FOR005 
$ WRITE SYSSOUTPUT ""X" 

$ GOTO TOP 
$ ! 

$ EXIT: 

$ EXIT 


I 


l 
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The default values of NAVPLT are defined in a value-setting 
file, SETVAL.INP. An example of this file is shown as Figure C2. 
As seen in the figure, the file is divided into sections, one for 
each main plot type (error, trajectory, residuals). Each section 
is divided into lines which contain the following fields: 

Field 1 . Line number : three numbers , separated by periods , 
are used by the program to indicate which plot 
parameters to access (do NOT change these numbers) 

Field 2 . Text : labels and parameter names 

Field 3. Parameter value: numeric value to be used as a 
limiting value for the plot . 

Each section contains the following elements: 


Element 1 . 


Title line: lines numbered 0.0.0 title the section 


Element 2. Use line: line number <n>.0.1 indicates to the 

program whether the user wishes to use the values 
in SETVAL.INP or to obtain the values interactively. 
A value of (1) indicates that the defined SETVAL 
values are to be used, while (0) will obtain the 
values interactively. 

Element 3. Information labels: if the third number is (0), 

(for example, 1.3.0), the line-entry is a label 


Element 4. Plot parameters: these lines contain an (■*) and a 

following number which defines the parameter's value. 
Min/max values must contain a decimal point, while 
indicators must not contain a decimal point. 


Numeric values which follow an («) must be preceded by a space. 
The min/max values for trajectory plots have been scaled down 
by 1000 to facilitate plot axis labeling. 
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Figure C2: SETVAL.INP Plot-parameter Default File 



* 


i 


0.0.0 ERROR PLOTS 

1.0. 1 USE =1 ! - 1 USE VALUES, *= 0 ASK FOR VALUES 

1.1.0 Maximum Y-axis values for Error plots 

1.1.0 Position Errors 

1.1.2 X-ERROR - 3002. 

1.1.3 Y-ERROR = 3002. 

1.1.4 Z-ERROR » 502. 

1.2.0 Velocity Errors 

1.2.1 XV-ERROR - 10. 

1.2.2 YV-ERROR = 10. 

1.2.3 ZV-ERROR = 10. 

0.0.0 TRAJECTORY PLOTS 

2.0. 1 USE =1 ! = 1 USE VALUES, = 0 USE DEFAULTS 

2.1.0 Max, Min, Delta for X-axis: PLOT #1 

2.1.1 MAX VALUE = 25. 

2.1.2 MIN VALUE - -25. 

2.1.3 DELTA VALUE - 5 . 

2.2.0 Max, Min, Delta for X-axis: PLOT #2 

2.2.1 MAX VALUE = 150. 

2.2.2 MIN VALUE = 0. 

2.2.3 DELTA VALUE = 15. 

2.3.0 Max, Min, Delta for Y-axis: PLOT #1 

2.3.1 MAX VALUE = 15. 

2.3.2 MIN VALUE - -10. 

2.3.3 DELTA VALUE = 5. 

2.4.0 Max, Min, Delta for Y-axis: PLOT #2 

2.4.1 MAX VALUE - 4 . 

2.4.2 MIN VALUE = 0. 

2.4.3 DELTA VALUE = 1. 

2.5.0 Marker every Nth point 

2.5.1 N - 60 

2.6.0 Marker symbol (DISSPLA marker values) 

2.6.1 SYMBOL = 15 
0.0.0 RESIDUAL PLOTS 

3.0. 1 USE - 1 

3.1.0 Maximum values for residual plots 

3.1.1 ALTITUDE = 220. 

3.1.2 GPS = 2800. 

3.1.3 DOPPLER = 9. 

3.1.4 AIRSPEED - 1 . 

3.1.5 POSITION FIX = -50. 

3.2.0 Minimum values for residual plots 

3.2.1 ALTITUDE = -310. 

3.2.2 GPS = -4300. 

3.2.3 DOPPLER = -12. 

3.2.4 AIRSPEED = -1. 

3.2.5 POSITION FIX = 50. 

7.7.7 END 



t 


C-5 




Program structure : — Examination of Figure Cl will show that 

NAVPLT is composed of four individual executable programs: 

1. RERRPLT for position and velocity error plots 

2. RTRJYPLT for trajectory plots 

3. RRESPLT for measurement residual plots 

4. DIPTEK for re-display of previously generated DIP files. 

These programs are called into execution through interactive 
control of the NAVPLT command procedure. Each of the first three 
programs reads its input from the list-files produced by NAVSIM 
II. 


navxxx . ER1 
navxxx . SGI 


RERRPLT 


CRT display 
DIP-file, ERRPLT . DIP 


navxxx . TR J 


navxxx . RES 


RTRJYPLT 



CRT display 
DIP-file. TRJYPLT.OIP 

CRT display 
DIP-file. RESPLT.dip 


Each of these three programs has the structure shown in the 
following sketch. 



list-fileCs) 


DIP files 


CRT plots 
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Reference Trajectory Utility 


An interactive program is included with NAVSIM II as a 
utility for helping the user to specify the reference trajectory. 
The trajectory generation routine, TAFCOS, requires 
trajectory-specification parameters in the particular format 
described in Section III and shown in Figure 4. The utility 
program. QNEW , was developed in order to calculate the required 
trajectory-specification parameters from responses to a set of 
appropriate prompts. QNEW enables en-route trajectories as well 
as terminal-area trajectories and provides for off-axis flight 
paths in the terminal area. It will find appropriate circular 
arcs and straight-line segments to connect different terminals. 
The output from QNEW is a file written to FOR010.DAT in the 
format required by TAFCOS. This file is not quite complete, 
however, to be used as the TRJXXX.INP file needed by NAVSIM II 
and must be edited somewhat . The editing requirements will be 
obvious when the user compares the FOR010.DAT file with the 
example trajectory-input file shown in Figure 4. 

The source-files for the various modules which comprise 
program QNEW are all found in a single module, QNEW. FOR. This 
module may thus be edited and then re-compiled and re-linked with 
the simple commands : 

$ FOR QNEW 
$ LINK QNEW 

The interactive dialogue with QNEW begins with: 

ENTER LAT-LNG AND X-AXIS HEADING OF YOUR COORDINATE SYSTEM DEG 

The purpose of this information is to enable translation and 
rotation of coordinate frames for en-route and 
terminal-to-terminal trajectories. If the entire trajectory is 
to be flown in the same coordinate system, the user's response to 
this prompt is irrelevant. Otherwise, he should respond with 
latitude (+North, degrees), longitude OEast, degrees) and x-axis 
heading (+CCW from North, degrees), separated with commas. 
Subsequent prompts ask for the various parameters which must be 
supplied in order to define each "leg" of the trajectory. The 
prompting sequence will vary according to the responses provided 
by the user . The program treats three types of flight segment : 

1 . local flight 

2. long-distance flight 

3. hover and ascent /descent . 

In any case, the user must supply the initial position (x,y,z) 
and ground speed. 



Local flight 


The program treats hoth straight-line segments and circular 
arcs. The inputs are: 


for straight lines 

either heading and distance to the end-point 
or x,y coordinates of the end-point 

altitude and speed at the end point 


for circular arcs 

either final heading at the end of the arc 
or fraction of a full circle to traverse 

radius and sense of the turn 

altitude and speed at the end point 

Long-distance flight 


The program determines circular arcs and straight-line 
segments to tangentially connect turning-circles at each end of 
the "leg." The two end-points may be specified in different 
coordinate systems (i.e., at different latitudes and longitudes 
and at different x-axis headings). This program feature 
implements some of the formulations found in [6] "Automation of 
Onboard Flight Management," NASA TM-84212 by H. Erzberger. The 
user is required to supply turn-radii at the initial and final 
points of the "leg." He may specify turn senses or let the 
program determine these so that the transfer path length is 
minimized. The final heading (i.e., at the end of the 
turn-straight-turn combination) may either be specified or else 
may be determined by the program to be that which will direct the 
aircraft toward the next (as yet unspecified) destination point. 

Hover and ascent descent 


The only specification for hover is the time duration. 
Ascent and descent require specification of final altitude as 
well . 


The path-generation feature is not as user-friendly as it 
could be made to be with additional effort, but it can be useful 
in assisting in construction of complex reference paths for input 
to TAFCOS . 
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Appendix D 
SUBROUTINE SUMMARIES 


This appendix contains a brief summary of each of the 
subroutines of NAVSIM II. The summarized subroutines appear in 
alphabetical order. Included in each summary are: 

1. a brief statement of the subroutine's purpose, 

2. a list of every communicated parameter which is 
set or used by the subroutine, 

3. a list of the routines which reference the subroutine 
being summarized, 


4. a list of the subroutines referenced by the subroutine 
being summarized. 



SUBROUTINE AIRSPEED 


c* 

c* 


************** 

* * * 

************************************ 

c* 

c* 

p * 

AIRSPEED - Models 

airspeed measurements (MTYPE=7) 

c* 

Intermodule 

Data : 



c* 

/ACTUAL/ 

ACTUAL_ASSF 

U 

True airspeed scale factor 

c* 

/MESCOM/ 

MINDEX 

S/U 

Measurement queue index 

c* 


MTYPE(IO) 

s 

Measurement -type code array 

c* 


H(22, 10) 

S/U 

Measurement sensitivity vector 

c* 


M(22, 10) 

s 

Identifying array for H 

c* 


MSTIMC 10) 

s 

Time measurement taken 

c* 


SIGMESC 10) 

s 

Measurement weight 

c* 


YRES(IO) 

s 

Measurement residual array 

c* 

/MESWTS/ 

PBIAS(IO) 

u 

Bias of measurement 

c* 


PGAUSS(IO) 

u 

Scale for random measurement noise 

c* 


PBADPCT (10) 

u 

Percentage of bad measurements 

c* 


PSCRNLMT(IO) 

u 

Threshold for screening bad points 

c* 


PMIN(IO) 

u 

Minimum acceptable measurement 

c* 


SIGM(IO) 

u 

Measurement weights 

c* 

/STATCM/ 

ASSFE 

u 

Airspeed scale factor estimate 

c* 


VN( 3) 

u 

Velocity vector 

c* 


WINDE ( 2 ) 

u 

Wind estimate 

c* 

/TIMCOM/ 

MTIME 

u 

Time oounter at filter rate 

c* 

/TRUTHC/ 

TVW ( 3 ) 

u 

True wind velocity 

c* 

n * 


V(3) 

u 

True velocity in Earth-fixed coords 

c* 

c* 

Called By : 

MEASIM 




C* Modules Referenced i BARN1 
C* 

Q* ************************************************************** 
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SUBROUTINE ALTMTR (KTYPE) 


£**********************************************«************** 

c* 

c* 

C* ALTMTR (KTYPE) - Models taro- or radio-altimeter 
C* 


c* 

Intermodule 

Data : 


c* 

c* 


KTYPE 

U 

c* 

/MESCOM/ 

MTYPE(IO) 

S 

c* 


H(22, 10) 

S 

c* 


MC22.10) 

S 

c* 


YRES(IO) 

S 

c* 


SIGMES(IO) 

S 

c* 


MSTIM(IO) 

S 

c* 


MINDEX 

s/u 

c* 

/MESWTS/ 

SIGM(IO) 

u 

c* 


PBIAS(IO) 

u 

c* 


PGAUSS(IO) 

u 

c* 


PBADPCT(IO) 

u 

c* 


PSCRNLMT(IO) U 

c* 


PMIN(IO) 

u 

c* 

/STATCM/ 

K(3) 

u 

c* 

/TIMCOM/ 

MTIME 

u 

c* 

c* 

/TRUTHC/ 

R(3) 

u 


C* Called By : MEASIM 

C* 


C* Modules Referenced : BARN1 
C 


If KTYPE=1 - Baro-altimeter 
If KTYPE-2 - Radio-altimeter 
Measurement type code 
Measurement sensitivity vector 
Identifying array for H 
Measurement residual 
Measurement weight 
Time measurement taken 
Measurement queue index 
Measurement weights 
Bias of measurement 
Scale for random measurement noise 
Percentage of bad measurements 
Threshold for bad points 
Minimum measurement for processing 
Estimated position vector 
Time counter at filter rate 
True position vector 


'iff* 


*********************************************************** 


SUBROUTINE APFILL 


c*,*******#*************,***********************,**********,, 

c* 

c* 

C* APFILL - To fill the transition matrix elements 
C* 

C* Intermodule Data : 

C* /TRANS/ AP(100) S Integral of AK 

C* AK(100) S/U Sum of the non-zero elements 

C* of the transition matrix 

C* 

C* Called By : TIMEUP 
C* 

C* Modules Referenced : NONE 
C* 

Q******************************************#********»******* 
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SUBROUTINE AUPT 


Q******************************************************************** 

c* 

c* 

C* AUPT - Calculates non-zero elements of the transition matrix 
C* AUPTIN - (Entry) Initializes constant values 
C* 


c* 

Intermodule 

Data : 



c* 

/AUPCOM/ 

TAUGYR ( 3 ) 

U 

Time constants for gyro noise, seconds 

c* 


TAUACB 

U 

Time constants for accelerometer noise. 

c* 

/CONST/ 

GO 

U 

Gravitational acceleration at sea level 

c* 

c* 

/EROTIC/ 

WEV( 3) 

U 

Earth's rotational rate vector 
in Earth-fixed coordinates, rad/sec 

c* 


RE 

U 

Earth ' s radius , ft 

c* 

/GRVCOM/ 

GVEC(3) 

U 

Gravity vector, ft /sec* *2 

c* 

/STATCM/ 

X(3) 

U 

Position vector, ft 

c* 


VN( 3) 

U 

Velocity vector, ft /sec 

c* 


TEB(3,3) 

U 

Estimated Earth to body transformation 

c* 

/TIMCOM/ 

DT 

U 

Integration step size, sec 

c* 

/TRANS/ 

AP(100) 

S 

Integral of AK 

c* 

c* 


AK(IOO) 

S/U 

Sum of the non-zero elements of the 
Jacobian matrix 

c* 


AM(IOO) 

s/u 

Non-zero elements of the Jacobian matrix 

c* 

c* 


NAP(IOO) 

s 

State index for the Jth non-zero element 
of the transition matrix 

c* 

c* 


NRAPC21) 

s 

Number of non-zero elements in the Ith 
row of the transition matrix 

c* 

c* 


NBEG(21) 

s 

Index of the first non-zero element in 
the Ith row of the transition matrix 

c* 

c* 

c* 

c* 

c* 

c* 

Called By : 

ORDER 

INESTM 

NAVEQ 

s 

Highest order for transition-matrix 
updating in subroutine PHIX 


C* Modules Referenced : NONE 
C* 

Q* ***************************************************************** * 
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REAL FUNCTION BARN1 (I , IKEY , IFRN) 


C* * 

C* 

C* 

C* 

C* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* * 


**************************************************************** 


BARN1 - random number generator 

Intermodule Data : 

I 

IKEY 
IFRN 


Local Data : 

SD The desired standard deviation 1. 

AMEAN The desired mean 0. 

H The population size 12. 


Called By : AIRSPEED 

ALTMTR 
DOPPLER 
FILNIT 
GPS 
INESTM 
INTRUE 
NAVEQ 
READINP 

Modules Referenced : NONE 

**************************************************************** 


U If I < 0 Gaussian distribution 
If I > 0 Uniform distribution 
U If IKEY < 0 Use compiled value for IFRN 
If IKEY > 0 U value transmitted in call 
U First random number 


SUBROUTINE CHNGX 


C 


c* 

c* 

************* 

********* 

******************************************* 

c* 

c* 

p * 

CHNGX - Puts 

estimated state 

changes into estimated state 

c* 

Intermodule 

Data : 



c* 

/DELCOM/ 

DELXC21) 

s/u 

Incremental state estimates 

c* 

/STATCM/ 

X(3) 

s/u 

Position vector 

c* 


VN(3) 

s/u 

Velocity vector 

c* 


TILTS(3) 

s/u 

Tilt vector 

c* 


GDRE ( 3 ) 

s/u 

Gyro drift rate estimates 

c* 


ACNBE 

s/u 

Vertical acceleration bias estimate 

c* 


CLKBE 

s/u 

Clock bias estimate 

c* 


CKBRE 

s/u 

GPS clock bias rate estimate 

c* 


ASSFE 

s/u 

Airspeed scale factor estimate 

c* 

p * 


WINDE(2) 

s/u 

Wind estimate 

L * 

c* 

p * 

Called By : 

KALFIL 



c* 

Modules Referenced : 

MVTRN 


c* 



TWIST 


c* 





c* 

********************** 

******************************************** 


D-7 



* 


c 


SUBROUTINE CN VLIN ( NB , B , NMAX , XOUT , NBAD ) 


c* 

c* 

************ 

******** 

* * * * 

****************************************** 


c* 

c* 

CNVLIN ( NB , B , NMAX , XOUT , NBAD ) - Accepts an ascii string and 

i 

r 

c* 




converts up to 6 numbers starting 


c* 

n * 




with = , and separated with commas 

1 

L * 

c* 

Intermodule 

Data : 




c* 


NB 

U 

Number of characters in input line 

1 

c* 


B 

U 

Input line 

i 

c* 


NMAX 

u 

Maximum number of values to be returned 

f 

c* 


XOUT 

s 

Returned result 


c* 

/I ic 


NBAD 

s/u 

Error flag 

V 

i 

c* 

Called By : 

READINP 



c* 





f 

c* 

Modules Referenced 

: NONE 

I 

c* 

c* 

************ 

******** 

* * * * 

****************************************** 

t 


I 

t 

l 


1 

i 

i 


t 


k 
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SUBROUTINE DOPPLER 


^* ************************************************************** * 
c* 


c* 

c* 

p * 

DOPPLER - Simulate Doppler 

measurements 

o 

c* 

Intermodule 

Data : 



c* 

/CONST/ 

D2R 

U 

Radian to degree conversion 

c* 

/MESCOM/ 

MTYPE(IO) 

S 

Measurement type code 

c* 




If MTYPE = 1 Baro-altimeter 

c* 




If MTYPE = 2 Radio-altimeter 

c* 




If MTYPE = 3 GPS pseudo range 

c* 




If MTYPE = 4,5,6 Doppler velocity 

c* 




If MTYPE = 7 Airspeed measurement 

c* 




If MTYPE - 8,9 Position fix 

c* 


H(22, 10) 

S/U 

Measurement sensitivity vector 

c* 


M(22 , 10) 

s 

Identifying array for H 

c* 


YRESC10) 

s/u 

Measurement residual array 

c* 


SIGMES(IO) 

s/u 

Measurement weight 

c* 


MINDEZ 

s/u 

Measurement queue index 

c* 


MSTIM(IO) 

s 

Time of measurement 

c* 

/MESWTS/ 

SIGM(IO) 

u 

Measurement weights 

c* 


PBIASC 10) 

u 

Bias of measurement 

c* 


PGAUSS(IO) 

u 

Scale for random measurement noise 

c* 

/STATCM/ 

VN(3) 

u 

Velocity vector 

c* 


DNTE ( 3 ) 

u 

Doppler antenna mounting estimate 

c* 


TEB(3 , 3 ) 

u 

Estimated Earth to body transformation 

c* 

/TIMCOM/ 

MTIME 

u 

Time counter at integration rate 

c* 

/TRUTHC/ 

V(3) 

u 

True velocity in Earth-fixed coords. 

c* 

p * 


ETB(3) 

u 

Earth to body transformation 

L» 

c* 

p * 

Called By : 

MEASIM 



V-/ 

c* 

Modules Required : BARN1 



c* 


CROSS 



c* 

c* 

- 

MVTRN 




Q* ******* ************ ****************************** X ************** * 
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SUBROUTINE ESTMERR 


C* 

C* 

C* 

C* 

C* 

C* 


***************************************************************** 


ESTMERR - This module computes 
and estimated states 


the difference between true 


c* 

Intermodule 

Data : 




c* 

/ACTUAL/ 

ACTUAL_GDR ( 3 ) 

U 

True gyro drift rate, rad/sec 


c* 


ACTUAL. ACNBC 3) 

U 

True accelerometer null bias, ft /sec**: 

2 

I 

c* 


ACTUAL GSF( 3) 

u 

True gyro scale factor errors 

c* 


ACTUAL.ASF ( 3 ) 

u 

True accelerometer scale factor errors 

f 

c* 


ACTUAL.CLRB 

u 

True GPS receiver clock bias, nanosec 


c* 


ACTUAL.CKBR 

u 

True clock bias rate, nano-sec 

1 

c* 


ACTUAL.DNT ( 3 ) 

u 

True doppler antenna mounting, radians 

I 

c* 


ACTUAL.ASSF 

u 

True airspeed scale factor 


c* 

/ESTERR/ 

DX(3) 

s 

Position estimation error in feet 

i 

c* 


DXDC3) 

s 

Velocity estimation error in radians 

r 

i 

c* 


DOM( 3) 

s 

Tilt estimation error in radians 

l 

c* 


CLKBER 

s 

Error in clock bias estimate 


c* 


GDRER ( 3 ) 

s 

Error in gyro drift rate estimates 

l 

c* 


ACNBER 

s 

Error in accelerometer null bias est . 

E 

c* 


CXBRER 

s 

Error in clock bias rate estimates 


c* 


DNTER ( 3 ) 

s 

Error in Doppler antenna mounting est . 

» 

c* 


ASSFER 

s 

Error in airspeed scale factor 

£ 

c* 


WINDERC2) 

s 

Error in wind estimate 


c* 

/STATCM/ 

X( 3) 

u 

Position vector 

i 

c* 


VN(3) 

u 

Velocity vector 


c* 


TILTSC3) 

u 

Tilt vector 

i 

c* 


CLKBE 

u 

Clock bias estimate 


c* 


GDRE ( 3 ) 

u 

Gyro drift rate estimate 

! 

c* 


ACNBE 

u 

Accelerometer null bias estimate 

i 

c* 


CKBRE 

u 

Clock bias rate estimate 


c* 


DNTE ( 3 ) 

u 

Doppler antenna mounting estimate 

r 

c* 


ASSFE 

u 

Airspeed scale factor estimate 

£ 

c* 


WINDE(2) 

u 

Wind estimate 


c* 

/ TRUTHC / 

R(3) 

u 

True postion vector in Earth-fixed 


c* 




coordinates, ft 


c* 


V(3) 

u 

True velocity vector in Earth-fixed 

I 

c* 

ri * 




coordinates, ft /sec 


c* 

p * 

Called By : 

KALFIL 



1 

c* 

Modules Referenced : MVTRN 




c* 

p * 


TWIST 



1 

c* 

************ 

**************** 

* * * 

******************************* 
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SUBROUTINE FILNIT 


Q****************************************************************** 

C* 

c* 


c* 

n * 

FILNIT - Initialize arrays 

and variables 

u 

c* 

Intermodule 

Data : 



c* 

/AUPCOM/ 

TAUGYR C 3 ) 

U 

Time constants for gyro noise, sec 

c* 


TAUACB 

U 

Time constant for accelerometer noise, 

c* 

/CONST/ 

GO 

U 

Gravitational acceleration 

c* 




ft/sec* *2 

c* 


D2R 

U 

Degree to radian conversion factor 

c* 


SPS2SPH 

S 

Seconds per second to seconds /hour 

c* 


FPS22UG 

S 

Feet per second squared to micro-g 

c* 


RPS2DPH 

S 

Radians per second to degrees/hour 

c* 


R2ASEC 

s 

Radians to arc-seconds 

c* 


RAT2PCT 

s 

Ratio to percent 

c* 

/DELCOM/ 

DELX(21) 

s 

Incremental state estimates 

c* 

/KALM/ 

NST 

s/u 

Number of states total 

c* 


NFF 

s 

Number of forcing functions 

c* 


IMAXVM 

s 

Dimension of WM 

c* 


UD(253) 

s/u 

U-D covariance matrix 

c* 


GAMMA(2I , 6) S 

Forcing function sensitivities 

c* 


QD(6) 

s 

Process, noise covariance 

c* 

/KALMIN/ 

USIGM(IO) 

u 

Measurement weights 

c* 


PDIAG0C21) 

u 

Inital state uncertainties 

c* 


QDIAG0C6) 

u 

Process noise uncertainties 

c* 


BIASM(IO) 

u 

Measurement bias scale 

c* 


GAUSSM(IO) 

u 

Scale of random measurement noise 

c* 


BADPCT(IO) 

u 

Percentage of bad measurements 

c* 


SCREEN_LIMIT( 10) U Threshold for had measurements 

c* 


AMIN(IO) 

u 

Minimum value for taking meas. 

c* 

/MESCOM/ 

MINDEX 

s 

Measurement queue index 

c* 

/MESWTS/ 

SIGM(IO) 

s 

Measurement weights 

c* 


PBIAS(IO) 

s 

Bias of measurement 

c* 


PGAUSSC 10) 

s 

Scale for random measurement noise 

c* 


PBADPCT(IO) S 

Percentage of bad points 

c* 


PSCRNLMT(IO) S 

Threshold for screening bad data 

c* 


PMIN(IO) 

s 

Minimum of acceptable measurements 

c* 

c * 

/TIMCOM/ 

DT 

u 

Integration step size, seconds 

c* 

Called By : 

MONTEC 




c* 

C* Modules Referenced : BARNl 
C* 

^^t************************************************t************ 
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SUBROUTINE 


FILSIM 


Q************************************************************* 

C* 

c* 

C* FILSIM - Filter simulation 

C* Called by TAFCOS 20 times per second 

C* If NTIME > 0 it calls NAVE© 


c* 

It 

calls 

MEASIM at 

c* 




c* 

Intermodule 

Data : 


c* 

/TIMCOM/ 

NTIME 

U 

c* 




c* 

Called By : 

TAFCOS 


c* 



c* 

Modules Referenced 

: KALFIL 

c* 



MEASIM 

c* 



NAVEQ 

c* 



OUTTRJ 

c* 




c* 

************* 

****** 

********* 


NTIME=0, 20,40 then KALFIL 

Time counter at integration rate 
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REAL 


FUNCTION GD0P*4(GU, I.J.K.L) 


Q***********,******,*************************,********,*,*********#* 


c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 


GDOPCGU, I, J.K.L) - 


Intermodule Data : 
GU ( 3 , 1 0 ) 
I, J.K.L 


Computes geometric dilution of precision for 4 
satellites in a real *4 FUNCTION 


U Matrix of unit slant -range vectors 
U Unit slant -range vectore are 

located in columns I,J,K,L of GU 


C* Called By : GPS 
C* SEESAT 

C* 

C* Modules Referenced : INVERT 
C* 

Q* ************************************* * ************ ********* ****** 
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SUBROUTINE GPS 

» 

*. 

Q*** *********** 

*************** 

************************************** 


C* 

C* GPS - Models 

P * 

pseudo-range measurements for GPS (MTYPE=3) 

t 

C* Intermodule 

Data : 




C* /ACTUAL/ 

ACTUAL_CLKB 

U 

True clock bias 

B 

C* /CONST / 

R2D 

u 

Radian to degree conversion 

P 

C* 

FTPKM 

u 

Feet per kilometer 


C* 

C 

u 

Speed of light, ft/sec 
Earth's rotation rate, rad/sec 


C* /EROTIC/ 

WE 

u 

» 

C* 

RE 

u 

Earth ' s radius , ft 

j 

C* 

TI2C0(3 , 3) 

u 

Inert ial-to-runway transformation 


C* /MESCOM/ 

MTYPE(IO) 

s 

Measurement type code 

I 

C* 

H(22, 10) 

s/u 

Measurement sensitivity vector 

! 

C* 

M(22, 10) 

s 

Identifying array for H 

c* 

YRES(IO) 

s/u 

Measurement residual array 

at 

c* 

SIGMES(IO) 

s 

Measurement weight 

f 

[ 

f 

c* 

MSTIM(IO) 

s 

Time of measurement 

c* 

MINDEX 

s/u 

Measurement queue index 


C* /MESWTS/ 

SIGM(IO) 

u 

Measurement weights 

f 

c* 

PBIAS(IO) 

u 

Bias of measurement 


c* 

PGAUSS(IO) 

u 

Scale for random measurement noise 

C* /ORBITS/ 

OINC 

u 

Orbital inclination 

I 

C* 

ASCNOD(6) 

u 

Ascending nodes 

1 

c* 

ANOMLY (6,6) 

u 

Mean anomalies 

M 

c* 

RORB 

u 

Orbital radius for GPS orbits 


c* 

ORBITAL_RATE 

u 

Ang. rate for sats. , rad/sec 

l 

c* 

UM 

s/u 

Grav. const., km**3/sec**2 

1 

c* 

EFEMER(3) 

u 

Ephem. errors feet 


c* 

RCA( 3 ,6,6) 

u 

Radial, cross, in-track ephemeris 

r 

c* 



errors, feet 

i 

C* /STATCM/ 

X(3) 

u 

Position vector 

C* 

CLKBE 

u 

Clock bias estimate 

} 

t 

C* /TIMCOM/ 

MTIME 

u 

Time counter at integration rate 

C* /TRUORB/ 

RSO(3.24) 

u 

Initial satellite position vector 

c* 

VS0C3.24) 

u 

Initial satellite velocity vector 

I 

c* 

CB(20 , 24) 

s/u 

Orbital parameters 

C* /TRUTHC/ 

R(3) 

u 

True position, feet 


C* /VISCOM/ 

I SAT 

s/u 

Current satellite number 


C* 

ELMIN 

u 

Minimum elevation angle 


c* 

GDOPC 

s 

Calculated GDOP 

t 

c* 

ILSAT 

s/u 

Index for ISATS to select ISAT 


c* 

NSEC CHNG_SATS U 

Time interval with fixed sat . set 

« 

c* 

Jk 

LSATS(4 , 20) 

u 

Array of visible satellites 

i 

c 

C* Called By : 

n * 

MEASIM 



- 

C* Modules Referenced : BARN1 



t 

C* 

DOT 




C* 

GDOP 



r 

C* 

MVTRN 



i 

c* 

STEPD 




c* 

c* 

VNORM 



« 

D-14 




*- 


SUBROUTINE INEREF 


Q* *************************************************************** * 


c* 

c* 

c* 

p * 

INEREF - Initiializes 

Earth 

rate vector and radius of curvature 

c* 

Intermodule 

Data : 



c* 

/CONST/ 

R2D 

U 

Radius to degree conversion 

c* 

/EROTIC/ 

WEV(3) 

U 

Earth's rotation rate vector 

c* 


WE 

u 

Earth's rotation rate 

c* 


REV(3) 

u 

Earth's radius vector 

c* 


RE 

s/u 

Earth's radius 

c* 


TI2C0(3 , 3 

) U 

Inert ial-to-runway transformation 

c* 

/REFCOM/ 

VLAT 

u 

Runway reference latitude 

c* 


VLON 

u 

Runway reference longitude 

c* 


RAZ 

u 

Runway azimuth 

c* 

p * 


GHAO 

u 

Initial Greenwich hour angle 

c* 

p * 

Called By : 

NAVSIM 



c* 

p * 

Modules Referenced : GTRN 


L» 

c* 

************* 

*************** 

********************************** 
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I 


SUBROUTINE INESTM 


\ 


c* 

c* 

************ 

**************** 

* * * * 

a******************************** 

1 

c* 

c* 

n * 

INESTM - Initialize estimation 

equations 

P 

Lr 

c* 

Intermodule 

Data : 




c* 

/ACTUAL/ 

ACTUAL GDR ( 3 ) 

U 

True gyro drift rate, rad/sec 


c* 


ACTUAL_ACNB ( 3 ) 

u 

True accelerometer null bias, 


c* 




ft /sec/sec 


c* 

/CONST/ 

R2ASEC 

u 

Radians to arc-seconds 


c* 

/ERRCOM/ 

ERRSTATEIC(21) 

u 

Initial input error conditions 


c* 


ERRGYR ( 3 ) 

u 

Accel, errors, sf, bias, random 

\ 

c* 


ERRACCC3) 

u 

Accel, errors, sf, bias, random 


c* 


ICERR 

u 

Error treatment flag 


c* 


I GRAND 

u 

Random gyro flag 


c* 


IARAND 

u 

Random accelerometer flag 

r 

i 

c* 

/STATCM/ 

ACNBE 

s 

Accelerometer null bias estimate 

1 

c* 


ASSFE 

s 

Airspeed scale factor estimate 


c* 


CLKBE 

s 

Clock bias estimate 

i 

c* 


CKBRE 

s 

Clock bias rate estimate 


c* 


DNTE ( 3 ) 

s 

Doppler antenna mounting estimate 

I 

c* 


GDRE ( 3 ) 

s 

Gyro drift rate estimate 


c* 


TILTSC3) 

s 

Tilt vector 

I 

c* 


VN(3) 

s 

Velocity vector 

% 

c* 


VINDE(2) 

s 

Wind estimate 


c* 


X(3) 

s 

Position vector 

} 

c* 

/TRUTHC/ 

R(3) 

u 

True position vector in 

1 

c* 




Earth-fixed co-ordinates, feet 


c* 


V(3) 

u 

True velocity vector in 

t 

c* 




Earth-fixed co-ordinates, ft /sec 

r 

l 

c* 

c* 

c* 

Called By : 

MONTEC 



f 


C* Modules Referenced 

: AUPTIN 

1 

C* 

BARN1 


C* 

NAVINIT 


c* 

TWIST 

i 

c* 


- 


0 ********************************************************* 


1 


t 


ft 
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t 



SUBROUTINE INTRUE 


c* 

c* 

**#**#*#**#* 

****************** 

: * * * 

******************************** 

c* 

c* 

p * 

INTRUE - Initialize all true 

conditions 

c* 

Intermodule 

Data : 



c* 

/ACTUAL/ 

ACTUAL_GDR ( 3 ) 

S 

True gyro drift rate, rad/sec 

c* 


ACTUAL ACNB ( 3 ) 

S 

True accel. null bias, ft /sec 

c* 


ACTUAL_GSF ( 3 ) 

S 

True gyro scale factors 

c* 


ACTUAL_ASF ( 3 ) 

S 

True accelerometer scale factors 

c* 


ACTUAL_CLKB 

S 

True GPS clock bias, nanosec 

c* 


ACTUAL CKBR 

S 

True clock bias rate, nano-sec 

c* 


ACTUAL_DNT ( 3 ) 

S 

True Doppler antenna errors, deg 

c* 


ACTUAL_ASSF 

S 

True airspeed scale factor 

c* 

/CONST/ 

FPS22UG 

U 

Feet per second to micro-g's 

c* 


RAT2PCT 

U 

Ratio to per cent 

c* 


RPS2DPH 

U 

Radians per second to deg. /hr 

c* 


SPS2SPH 

u 

Seconds /hr 

c* 

/ERRCOM/ 

ERRSTATEIC(21) 

u 

Initial input error conditions 

c* 


ERRGYR ( 3 ) 

u 

Gyro errors 1 sf, 2 bias, 3 rand 

c* 


ERRACC ( 3 ) 

u 

Accel . input errors 

c* 

p * 


ICERR 

u 

Input errors in measurement 

c* 

p * 

Called By : 

MONTEC 



c* 

Modules Referenced : BARN1 



c* 


TAFINIT 



c* 

p * 


TRUSAT 



c* 

************ 

****************** 

* * * 

***************************** 
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SUBROUTINE KALFIL 


C* 

C* 

c* 
c* 
c* 
c * 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 


************************************************************** 


KALFIL - Master driver for the KALMAN filter 

Calls TIMEUP, then calls MEASUP until MINDEZ-0 
Called by FILSIM once per second 


Intermodule Data : 
/KALM/ NST 


/MESCOM/ 


UD(253) 

MTYPE(IO) 


U 

U 

u 


Number of states total 

U-D covariance matrix - upper loaded 


If MTYPE-1 
If MTYPE-2 
If MTYPE=3 


Baro-altimeter 
Radio-altimeter 
GPS pseudo-range 
If MTYPE-4 ,5,6 : Doppler velocity meas. 
If MTYPE=7 : Airspeed measurement 
If MTYPE-8,9 : Position fix 
MINDEX S/U Measurement queue index 
/TIMCOM/ MTIME U Time counter at integration rate 

Called By : FILSIM 

Modules Referenced : CHNGX 

ESTMERR 

MEASUP 

OUTERR 

OUTRES 

TIMEUP 

UD2SIG 

*************************************************************** 


I 

I 


X 


t 

i 
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SUBROUTINE ME. A SIM 


C************************************************************,******* 

c* 

c* 

C* MEASIM - Simulates measurements for NAVSIM II. 

C * Called once per second by FILSIM 

C* 

C* Intermodule Data : 

C* /TIMCOM/ MTIME U Time counter at filter rate 

C* /MESKED/ MESINT(IO) U Measurement interval 

C* MESSTOPClO) U Measurement stop time 

C* MESTIMEC 10) S/U Current time for each meas. 

C* 

C* Called By : FILSIM 

C* 

C* Modules Referenced : AIRSPEED 
C* ALTMTR 

C* DOPPLER 

C* GPS 

C* POSFIX 

C* 

Q***»***************»********************************************** 
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■ 


K 


I 


SUBROUTINE MEASUP(MM) 


c* 

************ 

*********** 

* * * * 

**************************** 

$■ 

c* 

c* 

MEASUP(MM) 

- Processes 

measurement data and updates the 

I 

c* 


UD matrix 

and 

the state estimates 

r 

c * 
C* 

Intermodule 

Data : 



» 

C* 


MM 

U 

Measurement queue index 

j 

C* 

/DELCOM/ 

DELX(21) 

S/U 

Incremental state estimates 


c* 

/KALM/ 

NST 

U 

Number of states total 

i 

c* 


UD(253) 

S/U 

U-D covariance of uncertainty (R*6) 

i 

c* 

/MESCOM/ 

H(22, 10) 

u 

Measurement sensitivity vector 

c* 


M(22, 10) 

u 

Identifying array for H above 

? 

i 

c* 


YRES(IO) 

u 

Measurement residual array 

c* 


SIGMES(IO) 

u 

Measurement weights for the filter 

c* 

/PRTCOM/ 

PRTM 

u 

Extra-output print flag for meas. 


c* 

/TIMCOM/ 

MTIME 

u 

Time counter at filter rate 

[ 

c* 

c* 

c* 

Called By : 

KALFIL 



ft 

I 


C* Modules Referenced : TRIMAT 
C* UDMES 

C* 

****************************************************************** 


r 

h 

I 


i 


i 


i 


t 




fe 
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SUBROUTINE MONTEC 


Q****************«***************************************** 

c* 

C* MONTEC - Initialize variables at start of eaoh Monte Carlo pass. 
C* Scales initial errors 


c* 



c* 

Intermodule 

Data : 

c* 

/MESKED/ 

MESSTART(IO) U 

c* 


MESTIME(IO) S 

c* 

/TIMCOM/ 

MTIME S 

c* 


NTIME S 

c* 

/VISCOM/ 

ILSAT S 

c* 



c* 

Called By : 

NAVSIM 

c* 


c* 

Modules Referenced : FILNIT 

c* 


INESTM 

c* 


INTRUE 

c* 


OUTINIT 

c* 




Q* ****************************** 


Measurement start time (sec) 
Current next-measurement time 
Time counter at filter rate 
Time counter at integration rate 
Index for selecting 4 satellites 


******************************* 


SUBROUTINE NAVEQ 


q************************* 

C* 

c* 
c* 
c* 
c* 


************************************* 


NAVEQ - Integrate with spheroidal, rotating Earth 
NAVINIT - (Entry) Initialize internal variables for NAVEQ 


True gyro drift rate, rad/sec 
True accel. null bias, ft /sec* *2 
True gyro scale factor, unitless 
True accel. scale factor, unitless 
Gravitational acceleration ft /sec* *2 
Radius of the earth, feet 
Earth's rotation rate vector, rad/sec 
Estimated gravity vector, ft /sec* *2 
Estimated gyro drift rate, rad/sec 
Estimated earth-to-body transformation 
Estimated tilt vector, radians 
Estimated velocity, feet /second 
Estimated position, feet 
Integration time-step, seconds 
Time counter at filter rate (1/sec) 

Time oounter at integration rate (20/sec) 
True specific force vector, g's 
True earth-to-body transformation 
Angular velocity (body rate) vector 
Angular acceleration vector 


c* 

Intermodule 

Data : 


c* 

/ACTUAL/ 

ACT_GDR ( 3 ) 

U 

c* 


ACT_ACNB ( 3 ) U 

c* 


ACT GSF(3) 

U 

c* 


ACT ASF( 3) 

U 

c* 

/CONST / 

GO 

U 

c* 

/EROTIC/ 

RE 

U 

c* 


WEV(3) 

U 

c* 

/GRVCOM/ 

GVEC( 3) 

s/u 

c* 

/STATCM/ 

GDRE ( 3 ) 

u 

c* 


TEB( 3 , 3) 

S/U 

c* 


TILTS( 3) 

S/U 

c* 


VN (3) 

S/U 

c* 


X(3) 

S/U 

c* 

/TIMCOM/ 

DT 

U 

c* 


MTIME 

S 

c* 


NTIME 

U 

c* 

/TRUTHC/ 

A( 3) 

U 

c* 


ETB(3 , 3) 

u 

c* 


W 

u 

c* 

c* 


WD 

u 

c* 

c* 

c* 

Called By : 

FILSIM 

INESTM 


c* 

Modules Required : AUPT 

c* 


CROSS 

c* 


MVTRN 

c* 


TWIST 


I 




a************#*****!********************** 


£ 

i 
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PROGRAM NAVSIM 


C* 

C* 

C* 

C* 

C* 

C* 

C* 

C* 

C* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 

c* 


************************************************************ 


NAVSIM II EXECUTIVE : FILED AS NAVSIM. FOR 
Intermodule Data : 

/TIMEIN/ MNTCRL U Number of Monte Carlo passes 

/OUTCOM/ MCPASS S Current Monte Carlo pass 

Modules Referenced : INEREF 

MONTEC 

OUTCLSE 

READINP 

SEESAT 

TAFCOS 


********************************************************** 
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I 


SUBROUTINE OUTPUT % 

q************************************************************* 

C* OUTPUT - Control output t 

C* ENTRY OUTOPEN - Open output files 

C* ENTRY OUTINIT - Initialize at beginning of each Monte Carlo pass 
C* ENTRY OUTTRJ - Write trajectory data to output file 

C* ENTRY OUTRES - Write residual data to output file * 

C* ENTRY OUTERR - Write measurement errors to output files !' 

C* ENTRY OUTSIG - Write standard deviation of estimate for measurements 

C* ENTRY OUTSIGT - Write standard deviations of error for time update i 

C* ENTRY OUTCLSE - Close output files i 


c* 

Intermodule Data : 



i 

i 

c * 

/CONST/ 

FPS22UG 

U 

Feet per second squared to micro-g's 

c* 


R2ASEC 

U 

Radians to arcseconds 

t 

c* 


RAT2PCT 

U 

Ratio to percent 

f 

c* 


RPS2DPH 

U 

Radians per second to degrees per hour 

c* 


SPS2SPH 

U 

Seconds/second to seconds/hour 

\ 

c* 

/ESTERR/ 

ACNBER 

U 

Accelerometer null bias estimation error 


c* 


ASSFER 

U 

Airspeed scale factor estimation error 

f 

c* 


CKBRER 

U 

GPS clock bias estimation error, nanosec 

i 

c* 


CLKBER 

U 

GPS clock bias rate estimation error 

c* 


DOM ( 3 ) 

u 

Tilt estimation error 


c* 


DZ(3) 

u 

Position estimation error 

1 

c* 


DZDC3) 

u 

Velocity estimation error 

1 

c* 


DNTER ( 3 ) 

u 

Doppler antenna misalignment est. error 


c* 


GDRER ( 3 ) 

u 

Gyro drift rate estimation error 

f 

c* 


WINDERC2) 

u 

Wind velocity estimation error 

k 

c* 

/KALM/ 

UD (253) 

u 

Upper-loaded covariance matrix 


c* 


NST 

u 

Total number of states 

i 

c* 

/MESCOM/ 

MINDEX 

u 

Measurement queue index 


c* 


MTYPE(IO) 

u 

Measurement type oode 

« 

c* 


YRES(IO) 

u 

Measurement residuals 

i 

! 

c* 

/OUTCOM/ 

FILNM 

u 

Input filename. Character*40 

c* 


NB 

u 

Number of bytes in FILNM 

i 

c* 


TITLE 

u 

First line of input file 


c* 


DATBF 

u 

Date of run 

l 

c* 


TIMBF 

u 

Time of run 

L 

c* 


MCPASS 

u 

Monte Carlo pass number 


c* 

/TIMCOM/ 

MTIME 

u 

Time counter at filter rate 


c* 

/TRUTHC/ 

A( 3 ) 

u 

True specific force vector, g's 


c* 


R(3) 

u 

True position vector, feet 


c* 


V( 3) 

u 

True velocity vector, feet /sec 


c* 

/VISCOM/ 

I SAT 

u 

GPS satellite index 


c* 

p* 


GDOPC 

u 

Computed geometric dilution of precision 

ft 

c* 

Called By 

: NAVSIM 




c* 

FILSIM 



l 

c* 


KALFIL 




c* 


MONTEC 




c* 

p * 


READINP 



a 

c* 

Modules Referenced : 

UD2SIG 
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SUBROUTINE PHIX (X,M,H,MH, IFLAG) 


Q* ********************* 

c* 

C* PHIX(X,M,H,MH, IFLAG) 
C* 

C* 


*************************************** 

- Linear time-update routine multiplies 
vectors by the state transition matrix 


* c* 

c* 

Intermodule 

Data : 
X 

S/U 

Vector to be multiplied 

c* 


M 

U 

Identifying array for upper-loaded H 

1 c* 


H 

S 

Upper-loaded sensitivity vector 

i c * 


MH 

S 

Output -identifying array for H*Phi 

c* 


IFLAG 

U 

If IFLAG = 0 X(t) = PHI*X(o) 

1 c * 

® c* 

/STATEC/ 

NSC(21 ) 

U 

If IFLAG - 1 H(o) - H(t ) *PHI 
If NSC(I) = 1 Include the Ith state 

i c * 

c* 


NS(21 ) 

U 

If NSC(I) - 0 Exclude the Ith state 
Array of indices of included states 

1 c* 


NT 

U 

Total number of states included 

I c* 

/TRANS/ 

AP(IOO) 

U 

Transition matrix elements (non-diagonal) 

c* 


NAP(IOO) 

S 

State index for the Jth non-zero element 

1 c* 

1 c* 


NRAP(21) 

S 

of the transition matrix 

Number of non-zero elements in the Ith 

1 c* 

- c* 


NBEGC21) 

S 

row of the transition matrix 

Index of the first non-zero element in 

f c* 

1 c* 


ORDER 

S 

the Ith row of the transition matrix 
Highest order for transition-matrix 

c* 

1 c * 

1 c* 

Called By : 

TIMEUP 


updating in subroutine PHIX 


C* 

C* 

c* 


Modules Referenced : NONE 

*********************************************************** 


1 
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SUBROUTINE POSFIX 


Q* ********************************************************** 

c* 

c * 

C* POSFIX - Measurement for position fix (MTYPE=8,9) 

C* 

C* Intermodule Data : 

C* 

C* /MESCOM/ MINDEX S/U Measurement queue index 

C* H(22 , 10) S Measurement sensitivity vector 

C* M(22 , 10) S Identifying array for upper-loaded H 

C* MSTIM(IO) S Time (counter) of the measurement 

C* MTYPE(IO) S Measurement type (8,9 for position fix) 

C* SIGMES(IO) S Measurement weights 

C* YRES(IO) S Measurement residual vector 

C* /MESWTS/ SIGM(IO) U Measurement weights for each meas. type 

C* /STATCM/ x( 3) U True position vector, feet 

C* /TRUTHC/ R(3) U Estimated position vector, feet 


c* 

/MESCOM/ 

MINDEX 

s/u 

c* 


H(22, 10) 

s 

c* 


M(22, 10) 

s 

c* 


MSTIM(IO) 

s 

c* 


MTYPE(IO) 

s 

c* 


SIGMES(IO) 

s 

c* 


YRES(IO) 

s 

c* 

/MESWTS/ 

SIGM(IO) 

u 

c* 

/STATCM/ 

X( 3) 

u 

c* 

c* 

/TRUTHC/ 

R(3) 

u 

c* 

c* 

Called By 

: MEASIM 


c* 

c* 

Modules Referenced : 

BARN1 

c* 

*********** 

*********** 

***** 


i 


I 


k 
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n 

i 


1 

i 

? 

i 


I 


? 

i 


? 

i 


READINP 


Reads input 
appropriate 


data file and 
common arrays 


copies data to 


Intermodule 

/AUPCOM/ 


/CONST/ 


Data : 

TAUGYR ( 3 ) 

TAUACB 

GO 

D2R 

R2D 

FTPKM 

C 

FPS22UG 

R2ASEC 

RAT2PCT 

RPS2DPH 

SPS2SPH 


s/u 

s/u 

s 

s 

s 

s 

s 

s/u 

s/u 

s/u 

s/u 

s/u 


SUBROUTINE. READINP 

Q*********************************************************** 

c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 
c* 

C* /MESKED/ HESSTART(IO) S/U 

C* MESINTC 10) S/U 

C* HESSTOP(IO) S/U 

C* /ORBITS/ NORBS S/U 

C* OINC S/U 

C* ANOMLY (6,6) S/U 

C* ASCN0DC6) S/U 

C* NSP0RBC6) S/U 

C* EFEMER(3) S 

C* 

C* RORB S 

C* ORB IT AL_R ATE S 

C* UM S 


Time constants for gyro noise 
Time constants for accel. noise 
Gravity at sea level, ft /sec/sec 
Degrees to radians conversion 
Radians to degrees conversion 
Feet per kilometer 
Speed of light, feet/nanosec 
Feet/sec/sec to micro g's 
Radians to arcseconds 
Ratio to percent 
Radians /sec to degrees /hour 
Seconds /second to seconds /hour 
/ERRCOM/ ERRSTATEICC21) S/U Initial input error conditions 

Gyro scale, bias, random errors 
Accel, scale, bias, random errors 
Error treatment flag (const , random) 
Gyro noise flag, 0 for no noise 
Accelerometer noise flag 
Input scale percentage of bad meas. 
Input measurement biases 
Input random measurement noise scale 
Initial state uncertainties 
Process noise uncertainties 
SCREEN_LIMIT( 10) S/U Bad measurement threshold 
USIGM(IO) S/U Measurement weights 
AMIN(IO) S/U Minimum measurement for processing 


/KALMIN/ 


ERRGYRC3) 

ERRACC(3) 

ICERR 

I GRAND 

IARAND 

BADPCT(IO) 

BIASM(IO) 

GAUSSM(IO) 

PDIAG0C21) 

QDIAG0C6) 


S 

S 

S/U 

S 

S 

s/u 

s/u 

s/u 

s/u 

s/u 


Measurement start times 
Measurement interval 
Measurement stop time 
Number of satellite orbital planes 
Orbital inclination, degrees 
Mean anomalies of each satellite 
Ascending node of each orbit 
Number of satellites per orbit 
GPS ephemeris errors 
radial, cross-track, along-track 
Orbital radius for GPS, km 
Angular rate, rad/sec 
Gravitational const, km**3/sec**2 


D-27 
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READINP (continued) 


i 


l 


c* 

/OUTCOM/ 

DATBF 

s/u 

Date of this run. Character *9 

i 

c* 


TIMBF 

s/u 

Time of this run. Character *8 


c* 


FILNM 

s/u 

Input filename. Character*40 


c * 


NB 

s/u 

Number of bytes in FILNM 

« 

c* 


TITLE 

s/u 

First line of input file 


c* 

/PRTCOM/ 

PRTT 

s 

Extra-output flag for time update 


c* 


PRTM 

s 

Extra-output flag for meas. update 


c* 

/REFCOM/ 

GHAO 

s/u 

Initial Greenwich Hour Angle 

1 

c* 


RAZ 

s/u 

Runway azimuth (deg CW from North) 


c* 


VLAT 

s/u 

Runway latitude , deg 


c* 


VLON 

s/u 

Runway longitude , deg 

1 

c* 

/STATEC/ 

NSC(21) 

s/u 

States-considered flags 


c* 


NS( 21 ) 

s/u 

Upper-loaded s-c flags 


c* 

/TIMCOM/ 

NTMAX 

s 

Stop-time counter 

i 

c* 

/TIMEIN/ 

TALI 

s/u 

Time to start alignment 

f 

c* 


TAL2 

s/u 

Time to stop alignment 

1 

c* 


TFL1 

s/u 

Time to start filter 


c* 


TSSTOP 

s/u 

Stop time for the run 

f 

c* 


MNTCRL 

s/u 

Number of Monte Carlo passes 

1 

c* 

/VISCOM/ 

DESIRED GDOP 

s/u 

Desired value of GDOP 


c* 


ELMIN 

s/u 

Minimum elevation for GPS obs . 

K 

c* 


NSEC_CHNG_SATS 

S/U Time interval for fixed 

l 

c* 

n * 




4-satellite configuration 


c* 

Called By : 

NAVSIM 



\ 

c* 

Subroutines 

Referenced : 

BARN1 


c* 



CNVLIN 

l 

c* 



OUTOPEN 

i 

c» 



TIMEP 


c* 

c* 

************ 

************* 

* * * * * 

*************************************** 

i 


f 

i 


i 


i 


B 


* 


t 
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SUBROUTINE SEESAT 


c* 

************ 

************* 

****** 

****************************** 

c* 

c* 

p * 

SEESAT - Shows coverage : 

for a i 

stationary aircraft 

c* 

Intermodule 

Data : 



c* 

/CONST/ 

FTPKM 

U 

Feet per kilometer 

c* 


R2D 

U 

Radians to degrees conversion 

c* 

/EROTIC/ 

WE 

U 

Earth's rate, rad/sec 

c* 


REV(3) 

U 

Earth's radius vector, feet 

c* 

/ORBITS/ 

NORBS 

U 

Number of orbital planes 

c* 


OINC 

U 

Orbital inclination, degrees 

c* 


ANOMLY (6,6) 

U 

Mean anomalies , degrees 

c* 


ASCN0D(6) 

U 

Ascending nodes of each plane 

c* 


NSP0RB(6) 

U 

Number of satellites per orbit 

c* 


RORB 

S/U 

Orbital radius , km 

c* 


ORB I T AL_R ATE 

S/U 

Angular rate, rad/sec 

c* 


UH 

s/u 

Grav. const., km* *3/sec* *2 

c* 

/TIMEIN/ 

TSSTOP 

U 

Stop time for the run 

c* 

/VISCOM/ 

DESIRED_GDOP 

U 

Desired value for GDOP 

c* 


ELMIN 

u 

Minimum elevation for observing 

c* 


I SAT 

s 

Current satellite index 

c* 


NSEC_CHNG_SATS U 

Time interval with same 4 sats. 

c* 

p * 


LSATS(4, 20) 

s/u 

Array of visible satellites 

c* 

p * 

Called By : 

NAVSIH 



c* 

Subroutines 

Referenced : 

CROSS 


c* 



DOT 


c* 



GDOP 


c* 



VNORM 


c* 

c* 

************ 

************* 

****** 

******************************* 
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SUBROUTINE TAFCOS 


Q************************************************************* 

c* 

C* TAFCOS - Generates helicopter's "actual" trajectory, 

C* forces, body rates and attitude 

C* 

C* Entry TAFINIT - Initialization of initial conditions 
C* 

C* Intermodule data: 


c* 

/EROTIC/ 

WEV(3) 

U 

Earth's rotation rate vector. 

c* 




radians / second 

c* 


RE 

u 

Earth's radius in feet 

c* 

/OUTCOM/ 

FILTRJ 

u 

Trajectory-file name (characters) 

c* 


NTRJ 

u 

Number of characters in FILTRJ 

c* 

/TIMCOM/ 

NT I ME 

s/u 

Time counter, integration rate 

c* 


NTMAX 

u 

Stop time in counts 

c* 


DT 

s/u 

Integration step, seconds (DTP) 

c* 

/TRUTHC/ 

R(3) 

s/u 

True position vector, feet (ER) 

c* 


V( 3) 

s/u 

True velocity, ft /sec (EV) 

c* 


A( 3) 

s/u 

True specific foroe vector, 

c* 




in g's (FAB) 

c* 


ETB 

s/u 

(3,3) Earth-to-body transformation 

c* 




matrix (direction cosines) 

c* 


W(3) 

s/u 

Angular velocity vector, rad/seo 

c* 




(OMAA) 

c* 


WD(3) 

s/u 

Angular acceleration vector. 

c* 




rad /sec /sec (DOMAA) 

c* 

r% * 


TVW ( 3 ) 

s/u 

True wind velocity, ft /sec (WINDS) 

U 

c* 

Called by: 

NAVSIM 



c* 

p * 


INTRUE 

(initialization, TAFINIT entry) 

c* 

Modules referenced: 

CNVLIN 

c* 



FILSIM 

c* 



MVTRN 

c* 

n * 



TWIST 

c* * 

************* 

******** 

* * * * 

<********************************** 



SUBROUTINE TIMEUP 


c* 

c* 

************ 

********** 

****** 

**************************** 

c* 

c* 

TIMEUP - Does a linear time 

update for the estimated state and UD 

c* 

matrix. The 

result is stored in the W matrix, then WGS is 

c* 

n * 

called to retriangnlarize the W matrix into the UD matrix 

c* 

Intermodule 

Data : 



c* 

/DELCOM/ 

DELZ(21) 

s/u 

Incremental state estimates 

c* 

/KALM/ 

NST 

U 

Number of states total 

c* 


NFF 

U 

Number of forcing functions 

c* 


IMAXWM 

U 

Maximum dimension of WM matrix 

c* 


UD(253) 

S/U 

UD covariance matrix (real*8) 

c* 


WMC21.27) 

S/U 

Rectangular UD extended (real* 8) 

c* 


GAMMAC21.6) U 

Forcing-function sensitivity matrix 

c* 


QDC6) 

U 

Forcing-function variances 

c* 

/MESCOM/ 

H(22, 10) 

S/U 

Measurement sensitivities 

c* 


M(22, 10) 

U 

Identifying array for upper-loaded H 

c* 


MHC22, 10) 

S/U 

Post-multiplication M array 

c* 

/PRTCOM/ 

PRTT 

U 

Extra-output flag for time-update 

c* 

/STATEC/ 

NT 

S/U 

Number of states 

c* 

n ± 

/TIMCOM/ 

MTIME 

u 

Time counter at filter rate 

c* 

p * 

Called By ; 

KALFIL 



c* 

Modules Referenced : 

APFILL 


c* 



PHIZ 


c* 



TRIMAT 


c* 

c* 



WGS 


c* 

************ 

**************** 

**************************************** 
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SUBROUTINE TRIMAT (A , N, CAR , TEST , NCHAR , NAMES) 


q***************************************************************** 

C* 


c* 

TRIMAT (A , N, CAR , TEST , 

NCHAR, NAMES) - To display a vector-stored 

c* 

upper-triangular matrix in a two-dimensional triangular 

c* 

p * 

format 



c* 

Intermodular Data : 



c* 

A(N*(N+l)/2) 

U 

Vector containing upper triangular 

c* 



matrix 

c* 

N 

U 

Dimension of matrix 

c* 

CAR ( N ) 

U 

Parameter names 

c* 

TESTC ) 

U 

An array of field data characters to be 

c* 



printed as a title preceding the matrix 

c* 

NCHAR 

U 

Number of chars, to be printed in TEST( ) 

c* 



ABS(NCHAR) .LE. 126. -NCHAR is used 

c* 



to avoid skipping to a new page to print 

c* 

r* 

NAMES 

U 

.TRUE, to print parameter names 

c* 

Called By : MEASUP 



c* 

p * 

TIMEUP 



o 

c* 

p* 

Modules Referenced : 

NONE 

c* * 

******************** 

* * * 

*************************************** 



SUBROUTINE TRUSAT 


Q* * * * * * * * ******************************************************** * 

C* 

C* 

C* TRUSAT - Initializes "true" GPS satellite orbits as deviations 
C* from input circular orbits 

C* 

* Intermodule Data : 

Number of orbital planes 
Orbital inclination, degrees 
Initial mean anomalies, degrees 
Ascending nodes , degrees 
Number of satellites per orbit 
Ephemeris errors, radial, cross, 
along (ft) 

Randomized ephemeris errors by 
satellite 

Universal gravitational const., 
km* *3/sec* *2 


j c* 

c* 

Intermodule 

Data : 


/ORBITS/ 

NORBS 

U 

1 c * 

* c* 


OINC 

U 


ANOMLY (6,6) 

u 

1 c* 


ASCN0D(6) 

u 

c* 


NSP0RB(6) 

u 

i c * 

1 c* 


EFEMER( 3) 

u 

c* 

< c* 

i c* 

c* 


RCA(3 ,6,6) 

s 


UM 

u 

« c * 

/TRUORB/ 

CB(20 , 24) 

s/u 

] C* 


RSO(3 , 24) 

s/u 

I Q* 

c* 


VSO( 3,24) 

s/u 

f c* 

i c* 

Called By : 

INTRUE 


c* 

I c * 

c * 

c* 

Modules Referenced : 0RB2X 

STEPD 

X20RB 


c* 

] , 

************* 

************** 

* * * 
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i 



» 


t 


SUBROUTINE UD2SIG(U,N, SIG , TEXT , NCT) 


Q»t********************************************************** 

c* 

C* UD2SIG(U,N, SIG, TEXT, NCT) - Compute standard deviations 
C* (sigmas) from U-D oovariance 

C* 

C* Intermodule Data : 

C* U(N*(N+l)/2) U 

C* 

C* 

C* N U 

C* SIG(N) S/U 

C* TEXT( ) U 

C* 

C* NCT U 

C* 

c* 
c* 

C* Called By : KALFIL 
C* 

C* Modules Referenced : NONE 
C* 

Q* ^ ^ % t ************************************************************** * 


Input vector stored array containing 
the U-D factors. The D (diagonal) 
elements are stored on the diagonal 
Dimension of U and SIG 
Vector of output standard deviations 
Array of characters to be printed 
above the array of sigmas 
Number of characters in text . 

0 .LE. NCT .LE. 126 

If NCT ■= 0, no sigmas are printed 


v 


i 


1 

t 

\ 


\ 

i 

t 

» 


f 


* 
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ooooooooooooooooooooooooooooooooooooooooo 


SUBROUTINE UDMES (U , N , R , A , G , ALPHA , SCRLIM) 


COMPUTES ESTIMATE AND U-D .MEASUREMENT UPDATED 

COVARIANCE, P = UDU**T 

* * * INPUTS * * * 

U UPPER TRIANGULAR MATRIX, WITH D ELEMENTS STORED 

AS THE DIAGONAL. U IS VECTOR STORED AND 
CORRESPONDS TO THE PRIORI COVARIANCE. IF 
STATE ESTIMATES ARE COMPUTED, THE LAST COLUMN 
OF U CONTAINS X. 

N DIMENSION OF THE STATE ESTIMATES. 

R MEASUREMENT VARIANCE 

A VECTOR OF MEASUREMENT COEFFICIENTS, IF DATA 

THEN ACN+1) = Z 

ALPHA IF ALPHA LESS THAN ZERO NO ESTIMATES ARE 

COMPUTED (AND X AND Z NEED NOT BE INCLUDED) 


* * * OUTPUTS * * * 

U UPDATED, VECTOR STORED FACTORS AND ESTIMATE 

AND U((N+l)(N+2)/2) CONTAINS (Z-A* *T*X) /ALPHA 

ALPHA INNOVATIONS VARIANCE OF THE MEASUREMENT 
RESIDUAL 

G VECTOR OF UNWEIGHTED KALMAN GAINS, K=G/ ALPHA 

A CONTAINS U* *TA AND (Z-A* *T*X) /ALPHA 

Called by : MEASUP 
Modules Referenced : NONE 
C 
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■ 


I 


SUBROUTINE ¥GS ( ¥ , IMAX¥ , I¥ , J¥ , D , U , V ) 
*********************************************************** 

c* 

C* ¥GS ( ¥ , IMAXW , IW , JW , D , U , V ) - Modified Gramm-Schmidt algorithm for 
C* reducing ¥D¥(**T) to UDU(**T) form where U is a vector 

C* stored triangular matrix with the resulting D elements 

C* stored on the diagonal 

C* 


c* 

c* 

Intermodule Data : 
¥(I¥. J¥) 

s/u 

c* 

c* 

IMAX¥ 

U 

c* 

I¥ , J¥ 

U 

c* 

D(I¥) 

U 

c* 

c* 

c* 

U(I¥*(I¥+l)/2) 

S/U 

c* 

c* 

V(J¥) 

S/U 

c* 

c* 

c* 

Called By : TIMEUP 



Input matrix to be reduced to triangular 
format 

Maximum dimension of ¥ 

Actual dimension of ¥ 

Vector of non-negative weights for the 

orthogonalization process. The D's are 

unchanged by the calculation 

Output upper-triangular vector-stored 

matrix 

¥ork vector 


C* Modules Referenced : NONE 


C* 

Q* ****************************************************************** * 


% 


V 


I 


I 

I 

[ 

I 


I 

[ 


t 


k 

i 

I 


■ 


D-36 


■ 



Appendix E 
COMMONS DESCRIPTIONS 


This appendix contains a description of each of the commons 
used by NAVSIM II. The descriptions appear in alphabetical 
order . Included in each description are : 

1. a brief statement of the common's primary purpose, 

2. a list of the parameters which comprise the common, 
with mnemonic, dimension, definition and units, 

3. a set /used table which shows which subroutines access 
which parameters from the common. 


ACTUAL Labeled Common 


ACTUAL - Simulated truths in sensor models 


ACTUAL_GDR ( 3 ) 
ACTUAL_ACNB ( 3 ) 
ACTUAL_GSF ( 3 ) 
ACTUAL_ASF ( 3 ) 
ACTUAL_CLKB 
ACTUAL_CKBR 
ACTUAL_DNT ( 3 ) 
ACTUAL_ASSF 


True gyro drift rate, rad/sec 

True accelerometer null bias, ft/sec/sec 

True gyro scale factor error, unitless 

True accelerometer scale factor error, unitless 

True GPS receiver clock bias, nano-sec 

True clock bias rate, nano-sec 

True Doppler antenna mounting, radians 

True airspeed scale factor 


Accessing 

Subroutines : AIRSPEED ESTMERR GPS INESTM INTRUE NAVEQ 


ACTUAL_GDR ( 3 ) 
ACTUAL_ACNB ( 3 ) 
ACTUAL_GSF ( 3 ) 
ACTUAL_ASF ( 3 ) 
ACTUAL_CLKB 
ACTUAL_CKBR 
ACTUAL_DNT ( 3 ) 
ACTUAL_ASSF U 


U - U S U 

U - U S U 

S U 

- - S U 

U U - S 

U - - S - 

u - - s 

u - - s - 


Mathematical Symbols 
< 5 . ACTUAL GDR 

U 

f, ACTUAL ACNB 

b — 

s t . ACTUAL GSF 

s f ACTUAL_ASF 

c, ACTUAL CLKB 

b — 

£, ACTUAL CKBR 

b — 

OCj ACTUAL_DNT 

I , ACTUAL ASSF 

a/s — 
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AUPCOM Labeled Common 


AUPCOM - Time constants for inertial-sensor process noise 


TAUGYR ( 3 ) 

Array 

of time 

constants for gyro noise, sec 

TAUACB 

Time 

constant 

for accelerometer noise, sec 

Accessing 

Subroutines : 

AUPT 

FILNIT 

READINP 

TAUGYR 

U 

U 

S/U 

TAUACB 

U 

U 

S/U 


Mathematical Symbols 
TAUGYR 
~ TAUACB 


E-3 


K 


CONST Labeled Common 


CONST - Global constants 


GO 

Gravitational acceleration at sea level, ft /sec* *2 


D2R 

Degree to radian conversion factor 

V 

R2D 

Radian to degree conversion factor 


FTPKM 

Feet per kilometer 


C 

Speed of light, ft/nanosec 

i 

[ 

SPS2SPH 

Seconds per second to seconds per hour 

FPS22UG 

Feet per second squared to micro G's 

RPS2DPH 

Radians per second to degrees per hour 


R2ASEC 

Radian to arc seconds 

P 

RAT2PCT 

Ratio to percent 

t 


Accessing 
Subroutines : 

: AUPT 

DOPPLER 

FILNIT 

GPS 

INEREF 

INESTM 

NAVEQ 

GO 

U 

— 

U 

- 

- 

- 

U 

D2R 

- 

U 

U 

- 

- 

- 

— 

R2D 

- 

- 

- 

u 

U 

- 

- 

FTPKM 

- 

- 

- 

u 

- 

- 

— 

C 

- 

- 

- 

u 

- 

- 

— 

SPS2SPH 

- 

- 

S 

- 

- 

- 

- 

FPS22UG 

- 

- 

S 

- 

- 

— 

— 

RPS2DPH 

- 

- 

S 

- 

- 

- 

— 

R2ASEC 

- 

- 

S 

- 

- 

U 

— 

RAT2PCT 

- 

- 

s 

- 

- 

- 

— 



OUTPUT 

READINP 

SEESAT 


GO 

- 

S 

- 


D2R 

- 

S 

- 


R2D 

- 

S 

u 

Mathematical Symbols 

FTPKM 

- 

S 

u 


C 

- 

S 

- 

g GO 

& o 

SPS2SPH 

U 

S/U 

- 

C, C 

FPS22UG 

u 

S/U 

- 

1 

RPS2DPH 

u 

s/u 

- 


R2ASEC 

u 

S/U 



RAT2PCT 

u 

s/u 




I 

f 

l 

f 

I 

} 

t 


s 


t 
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DELCOM Labeled Common 


DELCOM - Incremental state estimates 

DELX(21) Incremental state estimates 

Accessing 


Subroutines : 

CHNGX 

FILNIT 

MEASUP 

TIMEUP 

DELX 

s/u 

S 

S/U 

S/U 


Mathematical Symbols 

A 

z, x DELX 
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EROTIC Labeled Common 


EROTIC - Earth's rotational rate, radius and inertial transformation 


WEV( 3) 

WE 

REV( 3) 

RE 

TI2C0(3 , 3) 


Earth's rotation rate vector in Earth-fixed 

coordinates, radians /second 

Earth's rotation rate in radians /second 

Earth radius vector in Earth-fixed co-ordinates, feet 

Earth's radius in feet 

Inertial transformation from Earth's equator-of-date 
to initial Earth-fixed runway coordinates 


Accessing 

Subroutines 

: ALTMTR 

AUPT 

GPS 

WEV 

_ 

U 

— 

WE 

- 

- 

U 

REV 

- 

- 

- 

RE 

U 

U 

u 

TI2C0(3,3) 

- 

- 

u 


INEREF NAVEQ 

S U 

S/U 
S 

S/U U 

S 


SEE SAT TAFCOS 

U 

U 

U 

U 


Mathematical Symbols 
c^. WEV 

R 
r 


REV 

RE 

TI2C0 



ERRCOM Labeled Common 


ERRCOM - Error state initial conditions 


ERRSTATEIC(21) 
ERRGYR ( 3 ) 
ERRACC ( 3 ) 

ICERR 
I GRAND 
IARAND 


Initial input error conditions 
Gyro scale factor, bias, random errors, deg/hr 
Accelerometer scale factor, bias, random 
errors, ft/sec**2 

Error treatment flag, 0 for constant, 1 random 
Gyro noise flag, 0 for no noise 
Accelerometer noise flag, 0 for no noise 


Accessing 


Subroutines : 

INESTM 

INTRUE 

NAVEQ 

READINP 

ERRSTATEIC(21) 

U 

U 

- 

s/u 

ERRGYR(3) 

- 

U 

U 

S 

ERRACC ( 3 ) 

- 

D 

U 

S 

ICERR 

U 

U 

- 

s/u 

IGRAND 

- 

- 

u 

s 

IARAND 

- 

- 

u 

s 


Mathematical Symbols 

z(0) 

ERRSTATEIC 


ERRGYR ( 1 ) 

/V 

lCJ 

b 

ERRGYR (2) 

(r 

wfioise 

ERRGYR (3) 

s f 

ERRACB(l) 

b bias 

ERRACB (2) 

°1:noise 

ERRACB (3) 
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ESTERR Labeled Common 


ESTERR - Estimation errors 


DX( 3) 

DXD ( 3 ) 
DOH(3) 
GDRER( 3) 
ACNBER 
CLKBER 
CKBRER 
DNTER (3) 
ASSFER 
WINDER (2) 


Position estimation error in feet 

Velocity estimation error in radians 

Tilt estimation error in radians 

Error in gyro drift rate estimates, deg /hr 

Error in accelerometer null bias estimates, ft/s2 

Error in clock bias estimate, nanosec 

Error in clock bias rate estimate, nanosec per sec 

Error in Doppler antenna mounting estimate, deg 

Error in airspeed scale factor estimate, unitless 

Error in wind estimate, feet per sec 


Subroutines : ESTMERR OUTPUT 


DX 

DXD 

DOM 

GDRER 

ACNBER 

CLKBER 

CKBRER 

DNTER 

ASSFER 

WINDER 


S 

S 

S 

S 

S 

S 

S 

s 

s 

s 


u 

u 

u 

u 

u 

u 

u 

u 

u 

u 


Mathematical Symbols 


X 

DX 



V 

n 

DXD 



P 

DOM 

r* 

S a/s 

ASSFER 

rs/ 

GDRER 

w 

V 

WINDER 

A/ 

f b 

ACNBER 



c b 

CLKBER 



o' 

c b 

CKBRER 



/v 

DNTER 





GRVCOM Labeled Common 


GRVCOM - Estimated gravity vector in earth-fixed coordinates 
GVEC(3) Gravity vector, ft/sec**2 
Accessing 

Subroutines : AUPT NAVEQ 

GVEC(3) U S/U 


Mathematical Symbols 
g GVEC 
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KALM Labeled Common 


i 


I 


I 


? 


KALM - Kalman Filter covariance parameters 


NST 

NFF 

IMAXWM 

UDC253) 

WM(21 , 27 ) 

GAMMA (21,6) 

QD(6) 


Number of states total 
Number of forcing functions 
Dimension of WM 

U - D covariance matrix - upper stored (Real *8) 
Time-updated UD matrix with process noise (Real*8) 
Forcing function sensitivity matrix 
Process noise covariance 


Subroutines : 

: FILNIT 

KALFIL 

MEASUP 

OUTPUT 

NST 

s/u 

U 

U 

U 

NFF 

S 

- 

— 

u 

IMAXWM 

s 

- 

— 

— 

UD 

s/u 

U 

s/u 

s/u 

WM 

- 

- 

— 

— 

GAMMA 

s 

- 

— 

— 

QD 

s 

— 

- 



TIMEUP 

U 

U 

U 

s/u 

u 

u 


Mathematical Symbols 

U,D UD 

W QD 

!D GAMMA 

n 


r 


\ 


1 

t 

S 

i 

I 

>• 

i 

! 


i 


t 


t 


I 
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KALMIN Labeled Common 


KALMIN - Kalman filter input parameters 


USIGMC 10) 

PDIAG0(21) 

<?DIAG0(6) 

BIASM(IO) 

GAUSSH(IO) 

BADPCT(IO) 

SCREEN_LIMIT (10) 

AMIN(IO) 


Input value of measurement weights 
Initial state uncertainties 
Process noise uncertainties 
Input value of measurement bias 
Input measurement value of random noise 
Input value of measurement points with 
bad values 

Input value of screening for bad 
measurement data 

Input value of minimum value for taking 
measurements ( e.g . airspeed > 30 Kts) 


NOTE : For USIGM, BIASM, GAUSSM, BADPCT , SCREEN_LIMIT and AMIN 



If 

I 


1 

Measurement 

is 

Baro- 

altimeter 


If 

I 

„ 

2 

it 

II 

Radio 

-altimeter 


If 

I 

_ 

3 

M 

II 

GPS 



If 

I 


4 

II 

II 

Doppler 


If 

I 

- 

5 

II 

II 

Airspeed 


If 

I 

= 

6 

II 

II 

Position fix 

Accessing 









Subroutines : 

FILNIT 

READINP 


Mathematical Symbols 

USIGM 


U 



s/u 


R 

USIGM 

PDIAGO 


u 



s/u 




QDIAGO 


u 



s/u 


D 0 

PDIAGO 

BIASM 


u 



s/u 




GAUSSM 


u 



s/u 


w 

QDIAGO 

BADPCT 


u 



s/u 




SCREEN_LIMIT 


u 



s/u 


B 

BIASM 

AMIN 


u 



s/u 


N 

GAUSSM 








K 

SCREEN_LIMIT 








^min 

AMIN 


E-ll 



MESCOM Labeled Common 


MESCOM - Measurement -processing parameters (queue for the filter) 


MTYPE(IO) 


H(22, 10) 
MH(22, 10) 
M(22, 10) 
YRES(IO) 
SIGMES ( 10 ) 
MSTIM(IO) 
MINDEX 


Measurement type code 

MTYPE - 1 : Baro-altimeter 

MTYPE - 2 : Radio-altimeter 

MTYPE = 3 : GPS pseudo range 

MTYPE = 4,5,6 : Doppler velocity measurement 

MTYPE - 7 : Airspeed measurement 

MTYPE - 8,9 : Position-fix x,y 


Measurement sensitivity vector - upper loaded 
Identifying array for H (output from PHIX) 
Identifying array for H (input to PHIX) 


Measurement residual array 
Measurement weight 
Time of measurement 


Measurement queue index 


Accessing 

Subroutines 

: AIRSPEED 

ALTMTR 

MTYPE 

S 

S 

H 

s/u 

S 

MH 

- 

- 

M 

s 

S 

YRES 

s 

s 

SIGMES 

s 

s 

MSTIM 

s 

s 

MINDEX 

s/u 

s/u 


MTYPE 

OUTPUT 

U 

POSFIX 

s 

H 

- 

s 

MH 

- 

- 

M 

- 

s 

YRES 

U 

s 

SIGMES 

- 

s 

MSTIM 

- 

s 

MINDEX 

u 

s/u 


DOPPLER 

FILNIT 

GPS 

XALFIL 

S 

_ 

S 

U 

S/U 

- 

S/U 

— 

S 

_ 

s 

- 

S/U 

- 

s/u 

- 

S/U 

- 

s 

- 

s 

- 

s 

- 

s/u 

S 

s/u 

s/u 


TIMEUP 

Mathematical Symbols 

s/u 

s/u 

H H 


U 

y YRES 

r SIGMES 

t MSTIM 


MEASUP 

U 


Cede! 


MESKED Labeled Common 


MESKED - Measurement schedule parameters 


MESSTART(IO) 

MESINT(IO) 

MESSTOP(IO) 

MESTIME(IO) 


Measurement start time 
Measurement interval 
Measurement stop time 

Current measurement time for each measurement type : 

MESTIME(l) - Next baro-altimeter measurement time 


MESTIMEC2) = 

“ radio-altimeter 

ii 

ii 

MESTIME( 3) = 

" GPS 

<i 

ii 

MESTIMEC4) = 

" Doppler 

«i 

ii 

MESTIMEC5) - 

" Airspeed 

ii 

ii 

MESTIME (6) - 

" Position-fix 

ii 



Accessing 

Subroutines : MEASIM MONTEC READINP 

MESSTART - U S/D 

MESINT U - S/D 

MESSTOP D - S/D 

MESTIME S/D S 


Mathematical Symbols 


t . 

MESSTART 

mi 

A- 

MESINT 

t . 

MESSTOP 

ml 

t 

m 

MESTIME 
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ME SWTS Labeled Common 


MESWTS - Measurement weights and simulated errors 


SIGM(IO) 

PBIAS(IO) 

PGAUSSC 10) 

PBADPCT(IO) 

PSCRNLMT(IO) 

PMINC10) 

NOTE : 


Measurement weights 
Bias of measurement 
Scale for random measurement noise 
Percentage of points with very bad values 
Limit for screening out bad data 
Minimum of acceptable measurements 
If I « 1 Measurement is Baro-altimeter 
If I = 2 " " Radio-altimeter 

If I - 3 " " GPS 

If I - 4,5,6 " " Doppler 

If i = 7 " " Airspeed 

If I - 8 9 " " Position fix 


Accessing 

Subroutines : AIRSPEED ALTMTR 


DOPPLER FILNIT 


GPS POSFIX 


SIGM U 

PBIAS U 

PGAUSS U 

PBADPCT 
PSCRNLMT 

PMIN U 


U U 

U U 

U U 


U 


S 

S 

S 

S 

S 

s 


u 

u 

u 


u 


Mathematical Symbols 

R SIGM 

B PBIAS 

N PGAUSS 

K PSCRNLMT 

y . PMIN 

'min 
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ORBITS Labeled Common 


ORBITS - GPS satellite orbit data 


NORBS The number of satellite orbital planes 

NSPORB(6) Number of satellites per orbit 

NSATS Total number of satellites 

OINC Orbital inclination, d eg 

ASCN0D(6) Ascending nodes, deg 

AN0MLY(6,6) Mean anomalies, deg 

RORB Orbital radius for circular GPS orbits, 1cm 

ORBITAL_RATE Angular rate for GPS satellite, rad/sec 

UM Gravitational constant, km**3/sec**2 

EFEMERC3) GPS ephemeris errors, ft radial, cross-, along-track 

RCA(3,6,6) GPS ephemeris errors by satellite, ft 


Subroutines : 

GPS 

READINP 

SEESAT 

TRUSAT 

NORBS 

_ 

S/U 

U 

U 

NSPORB 

- 

S/U 

U 

U 

NSATS 

- 

- 

- 

- 

OINC 

U 

S/U 

u 

u 

ASCNOt) 

U 

S/U 

u 

u 

ANOMLY 

u 

S/U 

u 

u 

RORB 

u 

s 

u 

- 

ORBITAL_RATE 

u 

s 

u 

- 

UM 

u 

s 

u 

u 

EFEMERC3) 

- 

s 

_ 

u 

RCA( 3,6,6) 

u 

- 

- 

s 


Mathematical Symbols 
i OINC 

O- ASCNOD 

ANOMLY 

M OR£ITAL_RATE 

JX UM 

e EFEMER, RCA 


E-15 


% 


Y 


l 

' 

Y 

OUTCOM Labeled Common , 


OUTCOM - Output file descriptors 


FILNM 

Output 

filename is FILNM plus file type 


NB 

Number 

of characters in 

FILNM 

T 

TITLE 

Title 

of output file 


i 

DATBF 

Date of current run 



TIHBF 

Time of current run 


| 

MCPASS 

Monte 

Carlo pass number 


i 

FILTRJ 

Trajectory filename 



NTRJ 

Number 

of characters in 

FILTRJ 

i 

Accessing 




i 

Subroutines 

: NAVSIM 

OUTPUT READINP TAFCOS 


FILNM 

_ 

U s/u 

- 

[ 

NB 

- 

U S/U 



TITLE 

- 

U S/U 

— 


DATBF 

- 

U S/U 

— 

! 

TIMBF 

- 

u s/u 

— 

■ 

MCPASS 

S 

U 

— 


FILTRJ 

- 

S 

u 

f 

NTRJ 

- 

S 

u 

i 


i 

E-16 

■ 


B 


PRTCOM Labeled Common 


PRTCOM - Extra-output print flags 


PRTT 

Time update 


PRTM 

Measurement 

update 

Accessing 

Subroutines 

: MEASUP READINP 

TIMEUP 

PRTT 

S 

U 

PRTM 

u s 

- 


E-17 


REFCOM Labeled. Common 


REFCOM - Earth reference parameters (constants for a run) 

Initial Greenwich hour angle, deg 
Runway reference latitude - North, deg 
Runway reference longitude - East, deg 
Runway azimuth clockwise from North, deg 

Accessing 

Subroutines : INEREF READINP 

GHAO U S/U 

VLAT U S/U 

VLON U S/U 

RAZ U S/U 


GHAO 

VLAT 

VLON 

RAZ 


Mathematical Symbols 

V GHAO 

0 

L VLAT 

r 


VLON 


STATCM Labeled Common 


STATCM - Current estimates of state 


X(3) 

VN( 3) 
TILTS C 3) 
GDRE ( 3 ) 
ACNBE 
CLKBE 
CKBRE 
DNTE ( 3 ) 
ASSFE 
WINDE ( 2 ) 
TEB(3 , 3) 


Position vector, feet 

Velocity vector, ft /sec 

Tilt vector, radians 

Gyro drift rate estimate, deg/hr 

Accelerometer null bias estimate, ft /sec* *2 

Clock bias estimate, nanosec 

Clock bias rate estimate, nanosec/sec 

Doppler antenna mounting estimate, deg 

Airspeed scale factor estimate 

Wind estimate, ft per second 

Estimated Earth to body transformation 


Accessing 
Subroutines : 

: AIRSPEED 

ALTMTR 

AUPT 

CHNGX 

DOPPLER 

ESTMERR 

GPS 

X 



U 

U 

S/U 

— 

U 

u 

VN 

U 

- 

U 

S/U 

U 

U 

- 

TILTS 

- 

- 

- 

S/U 

- 

U 

- 

GDRE 

- 

- 

- 

- 

- 

U 

- 

ACNBE 

- 

- 

- 

- 

- 

u 

- 

CLKBE 

- 

- 

- 

S/U 

- 

u 

u 

CKBRE 

- 

- 

- 

- 

- 

u 

- 

DNTE 

- 

- 

- 

- 

u 

u 

- 

ASSFE 

u 

- 

- 

S/U 

- 

u 

- 

WINDE 

u 

- 

— 

S/U 

- 

u 

- 

TEB 

- 

- 

U 

- 

u 

- 

- 


X 

INESTM 

S 

NAVEQ 

S/U 

POSFIX 

u 

Mathematical Symbols 

VN 

S 

S/U 

— 


TILTS 

s 

S/U 

- 

See ESTERR for X - WINDE 

GDRE 

s 

U 

- 

n 

ACNBE 

s 

- 

- 

, T TEB 

0 

CLKBE 

s 

- 

- 


CKBRE 

s 

- 

- 


DNTE 

s 

- 

- 


ASSFE 

s 

- 

- 


WINDE 

s 

- 

- 


TEB 

- 

S/U 

- 



E-19 



s 


IE 

* 

S 

STATEC Labeled Common \ 

STATEC - States-considered flags ? 

NSCC21) If NSC(I) = 1 : Include Itb state 

If NSC(I) = 0 : Exclude " __ , r 

NS(21 ) Upper loaded array of indices of included states and ff 

NT Total number of states included 

T 

Accessing 

Subroutines : PHIX READINP TIMEUP 

NSC U S/U - [ 

NS U S/U ' - * 

NT U - S/U 

\ 

I 

1 

I 

! 

I 
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TIMCOM Labeled Common 


TIMCOM - Time counters for program execution 


NT I HE 
MTIME 
NTMAX 
DT 


Time counter at integration rate 
Time counter at filter rate 
Stop time * 20 
Integration step size, sec 


Accessing 

Subroutines 


AIRSPEED ALTMTR AUPT DOPPLER FILNIT FILSIM GPS KALFIL 


NT I ME 
MTIME 
NTMAX 
DT 


U 


U 


U 


U 


U U 


U - U 


MEASIM 

NT I ME 

MTIME U 

NTMAX 

DT 


MEASUP 

MONTEC 

NAVEQ 

OUTPUT 

READINP 

TIMEUP 

TAFCOS 

_ 

S 

U 

- 

- 

- 

S/U 

U 

S 

S 

U 

- 

U 

— 


_ 


— 

S 

- 

U 



_ 

U 

- 

- 

- 

s/u 


E-21 



TIMEIN Labeled Common 


TIMEIN - Case-control times 


TALI 

TAL2 

TFL1 

TSSTOP 

MNTCRL 


Time to start alignment 
Time to stop alignment 
Time to start filter 
Stop time 

Number of Monte Carlo passes 


Accessing 

Subroutines : NAVSIM READINP SEESAT 


TALI 

- 

s/u 

TAL2 

- 

s/u 

TFL1 

- 

s/u 

TSSTOP 

- 

s/u 

MNTCRL 

U 

s/u 


U 


E-22 



< 


TRANS Labeled Common 


TRANS - Transition matrix parameters 

AP(IOO) Integral of AK, transition matrix elements (see APFILL) 

AK(IOO) Sum of the non-zero Elements of the transition matrix 

AM(100) Non-zero Elements of the transition matrix 

NAP(IOO) State index for the Jth non-zero element of the Ith row 
of the transition matrix 

NRAP(21) Number of non-zero elements in the Ith row of the 
transition matrix 

NBEGC21) Index of the first non-zero element in the Ith row 
of the transition matrix 

ORDER Highest order for transition-matrix updating for PHIZ 


Accessing 

Subroutines : APFILL AtJPT 


PHIX 


AP 

AK 

AM 

NAP 

NRAP 

NBEG 

ORDER 


Mathematical Symbols 
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TRUORB Labeled Common 


TRUORB - True GPS satellite orbital parameters 


RSOC 3 , 24) 
VSO( 3 , 24) 
CB(20 , 24) 


Initial position vector for each GPS satellite, feet 
Initial velocity vector for each GPS satellite, ft/sec f 
Orbital parameters for each GPS satellite j 


Subroutines : GPS TRUSAT 

RSO U S/U 

vso u s/u 

CB S/U S/U 


Mathematical Symbols 


R (0) 
s 

v (0) 
s 

E(0) 


RSO 

VSO 

CB 


E-24 


J 


TRUTHC Labeled Common 

TRUTHC - Simulated true (or "actual") aircraft state 


R(3) 

V( 3) 

A(3) 

ETB(3 , 3) 

W(3) 

WD(3) 

TVW ( 3 ) 

Accessing 

Subroutines 

R 

V 

A 

ETB 

W 

WD 

TVW 


True position vector in Earth-fixed co-ordinates, ft 
True velocity in Earth-fixed co-ordinates, ft /sec 
Specific force vector in aircraft-bodysco-ordinates , 
Earth to body transformation 
Angular velocity vector, rad/ sec 
Angular acceleration vector, rad/sec* *2 
True wind velocity, feet per second 


AIRSPEED ALTMTR DOPPLER ESTMERR GPS INESTM 


U 


U 


U 

D 


U 

U 


U 


U 

U 


U 


R 

V 

A 

ETB 

W 

WD 

TVW 


NAVEQ OUTPUT POSFIZ TAFCOS 


U 

U 

U 


U 


U 

U 

U 

U 


Mathematical Symbols 

x R 

v V 

n 

1 A 

/T V ETB 


S/U 

s/u 

s/u 

s/u 

s/u 

s/u 

s/u 


Mathematical Symbols 



W 

____ 


WD 


V 

w 

TVW 
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t 


* 



K 


VISCOM Labeled Common 

I 

VISCOM - GPS Visibility parameters 


Time interval with fixed set of 4 satellites 
Desired value of GDOP 

Minimum elevation angle satellite is visible 
Array of visible satellites 

Index for ISATS to select ISAT i 

Current satellite number being processed ( 

Calculated value of GDOP 

\ 


NSEC_CHNG_SATS 
DE SIRED JSDOP 
ELMIN 

LSATS(4 , 20) 
ILSAT 
ISAT 
GDOPC 


Accessing 

Subroutines : GPS 

NSEC_CHNG_SATS U 

DESIRED_GDOP 
ELMIN U 

LSATSC4.20) U 

ILSAT S/U 

ISAT S/U 

GDOPC S 


MONTEC OUTPUT READINP 

S/U 

S/U 

S/U 


U 

U 


SEESAT 

U 

U * 

U 

S/U -? 


f 

I 

\ 

F 

I 

* 

I 

$ 


ft 
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